RD-toTS  CM 


MN-SIMULMt  HOOCLIM  OF  RIGID  WMIPIUTORSfU)  MVI 
POSTQRflDURTE  SCHOOL  HOMTEREV  Cft  K  HOHAHHED  DEC  DC 


UNCLASSIFIED 


F/6  13/9 


NAVAL  POSTGRADUATE  SCHOOL 

Monterey,  California 


THESIS 


NON- SINGULAR  MODELING  OF 
MANIPULATORS 

RIGID 

by 

Khayyam  Mohammed 

December  1986 

Thesis  Advisor 

D.  L.  Smith 

Approved  for  public  release;  distribution  is  unlimited 


U  REPORT  SECuRiTy  CLASS'f 'CATiON 

1'NCLASSTFIFD 


a  SEiuRuy  Ci  ASSif  ifATiON  authority 


REPORT  DOCUMENTATION  PAGE 


'6  RESTRICTIVE  MARKINGS 


2b  DEtlASS'f 'CAT. ON  .  DOWNGRADING  SCntOUCt 


4  PERFORM. No  ORGANISATION  REPORT  NuM8£R;S> 


1  O1STR1BUT1ON/ AVAHASiLITY  Of  REPORT 

Approved  for  public  release; 

Hi  C  t  r  1  hilt  l  On  i  c  uni  imi  f  od 


S  MOMTOHiNG  ORGANISATION  RfcPQRT  NuV3t»l$) 


bj  NAM£  Of  PERFORMING  ORGANISATION  bo  OFFiCfc  $*MBOl  NAME  Of  MONiTORiNG  0«0AN»SAT. ON 

(if  4pph<4bl*)  I 

vnval  Post craduat e  School  69  Naval  Postgraduate  School 


6<  ADDRESS  iC/fy  Staff  sod  ftPCodt) 

Mo ntrrev,  California  959-33-5100 


Tb  AOOR£SS(Cify  Sure  end  HP  Coat) 


8a  NAME  Of  fuNOiNG  i  SPONSORING 
ORGANIZATION 


N,onterov,  California  9  5  0  4  3  -  5 1  n  n 


8D  Office  SYMBOL  9  PROCUREMENT  INSTRUMENT  lOEN'  f  CAT, on  NUMBER 
(it  tpplictblt) 


8c  ADORE SS  iCify  Sure  tnd /ip  Coat) 

|  ’0  SO'jRCf  O'  fliNDiNG  NUMBERS  | 

PROGRAM 

pro.ect 

*AS< 

A  OR*  ^NtP 

element  no 

NO 

NO 

ACCESSION  NO 

'  ' .  i  nnHoat  Itcw-ry  CuUitKtnon) 

NON  -  -c  IN'CHI.AP  ''OPFLI.  INT  OF  Rrorn  ma\;tpi't  .uorc 


PERSONA.  AuThORiSI 

Mohammed  .  Khawa 


■  )b  ’’ME  COVERED 
f  ROM  TO 


1  ] j  -*Pt  O'  REPORT 

"asters  Thes  i  s 


'4  DATE  O'  REPORT  lY»ar  Month  Day)  'S  fait  CD-.NT 
TOCk  Tlnrm-Kr-  ^0 


COSAT.  CODES 


SuB  G»OU'P 


'8  SUBJECT  TERMS  'Continue  on  rtvtnt  it  ntrtistry  and  identify  by  b'cik  r^motr) 

‘-Robot  ics  ,  Robot  ,  Simulat ion , S i ngul  ar  i  tv  ,"ode  1  i  ng  /. 
Dvnamic  equations  of  Mot  ion ,  Free  bod\-  analysis*, 


9  A8STRACr  'Continue  on  reverie  it  neceuery  and  identify  by  plotk  number) 

.>  A  problem  arises  when  conventional  kinematic  equations  that  minimize 
computational  time  are  used  to  model  a  rigid  revolute  robot  arm. 
Mathematical  singularities  result  when  successive  link  axes  ""line  up'“ 
such  that  their  angles  are  0  or  ISO  degrees.  This  may  result  m  erratic 
and  uncontrollable  motion  of  the  arm  until  if  moves  away  from  the  point 
cf  singularity.  One  solution  is  to  spend  a  minimum  amount  of  time  at 
the  singular  position  or  to  avoid  this  position,  alt:  nether .  .-.nether 
solution  is  to  use  other  sets  cf  equations,  instead  of  the  regular 
resolved-rate  equations,  tc  model  the  robot  arm.  This  tl.es  is  shews  how 
using  equations  based  on  Newton's  Second  Principle  cf  dynamics  for  a 
three  link,  two  degree  of  freedom.  manipulator,  the  problem  of 
singularity  is  avoided.  The  equations  are  demonstrated  m  a  simulation 
tore  cram.  '  ,  ' 


_  A.i.e  a 

Ipi'i-’i-O, 


:ne  reou.ar 


problem  Of 
a  oimulatio n 


.0  D  S  R 

O  ‘.Cl 

3  „  ’  ON  Av A  lABil'Ty' Of  ABSTRACT 

ASS  *  (DIM  M'T(0  Q  Same  AS  RRT  O  OTiC  USERS 

Zi  ABSTRACT  SECuRity  c^AyS'f'CA 

|'W!  acc  j  i  t  t  [i 

'ON 

■Effi 

0‘  RESPONS  BlE  ND'V  OuAl 

Jib  T  [  L  i  PhON£  {irxlua*  AfSSiOdti 

*  1  ■'  v  0  - 

U‘c  v  - »  '  :  )>VB.:. 

6  9  Sri 

DO  FORM  M73.  8a  mab 


8)  APB  tb  t  0"  T-Jy  Ot  u\IO  VM  i»«B»ulI»0 
An  orr-t'  *0  I  ON  »'t  OtMO'Btt 


sk.R'Ty  Class  in'  ii  --spa. 


Approved  for  public  release;  distribution  is  unlimited. 
Non-Singular  Modeling  of  Rigid  Manipulators 

by 

Khayyam  Mohammed 
Lieutenant.  United  States  Navy 
B.A.  in  Psychology  ,  Queens  College,  1977 

Submitted  in  partial  fulfillment  of  the 
requirements  for  the  degree  of 

MASTER  OF  SCIENCE  IN  MECHANICAL  ENGINEERING 

from  the 

NAVAL  POSTGRADUATE  SCHOOL 
December  1986 


ABSTRACT 


A  problem  arises  when  conventional  kinematic  equations 
that  minimize  computational  time  are  used  to  model  a  rigid 
revolute  robot  arm.  Mathematical  singularities  result  when 
successive  link  axes  "line  up"  such  that  their  angles  are  0 
or  180  degrees.  This  may  result  in  erratic  and 
uncontrollable  motion  of  the  arm  until  it  moves  away  from 
the  point  of  singularity.  One  solution  is  to  spend  a 
minimum  amount  of  time  at  the  singular  position  or  to  avoid 
it  altogether.  Another  solution  is  to  use  other  sets  of 
equations,  instead  of  the  regular  resolved-rate  equations, 
to  model  the  robot  arm.  This  thesis  shows  how  using 
equations  based  on  Newton's  Second  Principle  of  dynamics 
for  a  three  link,  two  degree  of  freedom  manipulator,  the 
problem  of  singularity  is  avoided.  The  equations  are 
demonstrated  in  a  simulation  program. 
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COMPUTER 

SYMBOL 

TEXT 

VARIABLE 

DESCRIPTION 

A 

A 

Sine  wave  input  torque  data 
aepl itude 

AA 

Aa 

Acceleration  of  point  a 

AB 

Ab 

Acceleration  of  point  b 

AG1 

Agl 

The  acceleration  vector  of 
the  center  of  gravity  for 
link  1 

AG  2 

Ag2 

Sane  as  Agl  but  for  link  2 

AG  3 

Ag3 

Sane  as  Agl  but  for  link  3 

AOX 

aoz 

Linear  acceleration  of  link 
zero  in  the  z  direction 

AO  Y 

aoy 

Linear  acceleration  of  link 
zero  in  the  y  direction 

AOZ 

aoz 

Linear  acceleration  of  link 
zero  in  the  z  direction 

AX1 

azl 

Linear  acceleration  of  link 
1  in  the  z  direction 

AY1 

ayl 

Linear  acceleration  of  link 
1  in  the  y  direction 

AZ1 


azl 


Linear  acceleration  of  link 
1  in  the  z  direction 


COMPUTER 

SYMBOL 

TEXT 

VARIABLE 

AX2 

ax2 

AY2 

ay  2 

AZ2 

az2 

AX  3 

ax  3 

AY3 

ay3 

AZ3 

az3 

CTHETX ( 3 ) 

C0X 

CTHETY ( 3 )  C0y 

CTHETZ ( 3 )  C6z 

DEGRA 

ETHETXO)  ex 

ETHETYO)  ey 

ETHETZ ( 3 )  Qz 


DESCRIPTION 


Linear  acceleration  of  link 
2  in  the  x  direction 

Linear  acceleration  of  link 
2  in  the  y  direction 

Linear  acceleration  of  link 

2  in  the  z  direction 

Linear  acceleration  of  link 

3  in  the  x  direction 

Linear  acceleration  of  link 
3  in  the  y  direction 

Linear  acceleration  of  link 
3  in  the  z  direction 

A  1x3  vector  Cartesian 
value  of  the  angle  theta 
for  link  1-3  in  the  x 
direction,  results  from 
taking  the  integral  of 
angular  acceleration  in  the 
x  direction  twice,  in 
degrees 

Same  as  c0x  but  in  the  y 
direction 

Same  as  c0z  but  in  the  z 
direction 

Conversion  from  degrees  to 
radians 

A  1x3  vector  of  euler 
angles  for  link  1-3  in  thex 
direction,  in  degrees 

Same  as  0x  but  in  the  y 
direction 

Same  as  8x  but  in  the  z 
direction 


COMPUTER 

SYMBOL 

TEXT 

VARIABLE 

DESCRIPTION 

EULORYO) 

they  3 

Theoretical  Euler  angle  for 
link  3  in  the  y  direction, 
in  degrees 

ERROR ( 3 ) 

Error ( 3) 

%  error  between  thdy3  and 
6y  for  the  third  link  in 
the  y  direction 

ERRT1X 

Errtlx 

\  error  between  computed 
and  input  value  of  torque 
at  joint  1 

ERRT2X 

Err t2x 

Same  as  Errtlx  but  at  joint 
2 

FXO 

Fxo 

Computed  force  in  the  x 
direction  at  joint  0 

FYO 

Fyo 

Computed  force  in  the  y 
direction  at  joint  0 

FZO 

Fzo 

Computed  force  in  the  z 
direction  at  joint  0 

FX1 

Fxl 

Computed  force  in  the  x 
direction  at  joint  1 

FY1 

Fyl 

Computed  force  in  the  y 
direction  at  joint  1 

FZ1 

Fzl 

Computed  force  in  the  z 
direction  at  joint  1 

FX2 

Fx2 

Computed  force  in  the  x 
direction  at  joint  2 

FY2 

Fy2 

Computed  force  in  the  y 
direction  at  joint  2 

FZ2 

Fz2 

Computed  force  in  the  z 
direction  at  joint  2 

G 
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Gravitational  constant 


COMPUTER 

SYMBOL 

TEXT 

VARIABLE 

DESCRIPTION 

HDX( 2 ) 

HDx 

The  tine  rate  of  change  of 
angular  momentum  of  a  2 
element  composite  body  in 
the  x  direction 

HDY( 2) 

HDY 

Same  as  HDx  but  in  the  y 
direction 

HDZ ( 2 ) 

HDz 

Same  as  HDx  but  in  the  z 
direction 

I 

Counter 

IA 

Row  diaension  of  matrix  A 
and  matrix  B  used  in  LEQT2F 
subroutine 

IER 

Error  parameter  used  in 

subroutine  LEQT2F 

IDGT 

Accuracy  test  used  in 
subroutine  LEQT2F,  for 
iterative  improvement 

IXX(3,2) 

Ixx 

A  3x2  matrix  of  Moment  of 
Inertia  for  the  two  element 
composite  body  of  link.  1-3 
about  the  x  axis 

I YY( 3, 2 ) 

Iyy 

Same  as  Ixx  but  about  the  y 
axis 

IZZ ( 3, 2 ) 

Izz 

Same  as  Ixx  but  about  the  z 
axis 

IXZ (3,2) 

Ixz 

A  3x2  matrix  of  Products  of 
Inertia  for  the  two  element 
composite  body  of  link  1-3 
about  the  xz  coordinate 
axes 

IXY ( 3,2) 

Ixy 

Same  as  Ixz  but  for  the  xy 
axes 

I YZ( 3, 2  ) 

Iyz 

Same  as  Ixz  but  for  the  yz 

axes 


COMPUTER 

SYMBOL 

TEXT 

VARIABLE 

IXXA( 3,2) 

Ixxa 

JXO 

jxo 

JYO 

jyo 

JZO 

jzo 

JX1 

jxl 

JY1 

jyi 

JZ1 

jxl 

JX2 

1x2 

JY2 

iy2 

JZ2 

j  z2 

L( 3, 2 ) 

L( 3, 2 ) 

LCOGX ( 3 ) 

LCOGx 

LCOGY ( 3 ) 

LCOGy 

LCOGZ ( 3 ) 

LCOGx 

DESCRIPTION 


Theoretical  Noient  of 
inertia  for  link  3  about 
joint  2 

Location  of  joint  0  in  the 
x  direction 

Location  of  joint  0  in  the 
y  direction 

Location  of  joint  0  in  the 
z  direction 

Location  of  joint  1  in  the 
x  direction 

Location  of  joint  1  in  the 
y  direction 

Location  of  joint  1  in  the 
z  direction 

Location  of  joint  2  in  the 
x  direction 

Location  of  joint  2  in  the 
y  direction 

Location  of  joint  2  in  the 
z  direction 

A  3x2  Matrix  that  is  the 
distance  froe  center  of 
link  to  center  of  mss  at 
each  link  end 

A  1x3  location  of  center  of 
gravity  vector  for  link  13 
in  the  x  direction 

Sa»e  as  LCOGx  but  for  the  y 
direction 

Same  as  LCOGx  but  for  the  z 
direction 
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COMPUTER 

SYMBOL 

M 

TEXT 

VARIABLE 

DESCRIPTION 

Used  in  LEQT2F  subroutine 
as  number  of  right  hand 
sides 

MASS( 3,2) 

Mass( 3, 2) 

A  3x2  matrix 
each  element 
the  composite 
1-3 

of 

that 

body 

mass  of 
make  up 
for  link 

MASS1 

Ml 

Total  mass  of 

link 

1 

MASS2 

M2 

Total  mass  of 

link 

2 

MASS3 

M3 

Total  mass  of 

link 

3 

SJ 


MATA<27,27) 


NATBC27) 


MatA 


MatB 


MI 


r: 

m 

MJ 

MK 

J* 

N 

Ur 

B 

MIAO 

& 

MXAO 

MIBO,  MJBO  and 
MXBO 


A  27x27  matrix  consisting 
of  coefficients  of  the 
unknown  variables 

A  1x27  vector  consisting  of 
the  coefficient  of  known 
variables  on  input  to 
subroutine  LEQT2F  and  an 
output  the  solution  to  the 
linear  equations 

Results  from  subroutine 
CPROD,  i  component  of 
vector  cross  product 

j  component  of  vector 
cross  product 

k  component  of  vector 
cross  product 

Cross  product  between  wdl 
and  RB/G1  at  joint  0, 
link  1,  in  the  x,  y ,  z 
direction 

Cross  product  between  wl 
and  RB/G1  at  joint  0,  link 
1,  in  the  x,  y,  z  direction 


COMPUTER 

SYMBOL 

TEXT 

VARIABLE 

DESCRIPTION 

MICO, 

MKCO 

MJCO 

and 

Cross  product  between  wl 
and  MI BO,  MJBO  and  MKBO  at 
joint  0,  link  1,  in  the  x, 
y,  z  direction 

MIA1, 

MKA1 

MJA1 

and 

Cross  product  between  wdl 
and  RA/G1  at  joint  1,  link 
1,  in  the  x,  y,  z  direction 

MIB1, 

MKB2 

MJB1 

and 

Cross  product  between  wl 
and  RA/G1  at  joint  1,  link 
1,  in  the  x,  y,  z  direction 

MIC1, 

MIC1 

MJC1 

and 

Cross  product  between  wl 
and  MIB1,  MJB1  and  MKB1 
respectively  at  joint  1, 
link  1,  in  the  x,  y,  z 
direction 

MIA2, 

MKA2 

MJA2 

and 

Cross  product  between  wd2 
and  RB/G2  at  joint  1,  link 
2,  in  the  x,  y,  z  direction 

MIB2, 

MKB2 

MJB2 

and 

Cross  product  between  w2 
and  RB/G2  at  joint  1,  link 
2,  in  the  x,  y,  z  direction 

MIC2, 

MKC2 

MJC2 

and 

Cross  product  between  w2 
and  MIB2,  MJB2  and  MKB2 
respectively  at  joint  1, 
link  2,  in  the  x,  y,  z 
direction 

HI  A3, 
MJA3 

MJA3 

and 

Cross  product  between  wd2 
and  RA/G2  at  joint  2,  link 
2,  in  the  x,  y,  z  direction 

MIB3, 

MKB3 

MJB3 

and 

Cross  product  between  w2 
and  RA/G2  at  joint  2,  link 
2,  in  the  x,  y,  and  z 
direction 

COMPUTER  TEXT 

SYMBOL  VARIABLE 

MIC3,  MJC3  and 
MXC3 


MIA4,  MJA4  and 
MKA4 


MIB4,  MJB4  and 
MKB4 

MIC4,  MJC4  and 
MKC4 


N 

P 

RUN 

RX( 3, 2 )  Rx( 3, 2) 

RY ( 3, 2 )  Ry(3,2) 

RZ( 3, 2  )  Rz( 3, 2 ) 


DESCRIPTION 


Cross  product  between  w2 
and  MIB3,  MJB3  and  MKB3 
respectively  at  joint  2. 
link  2,  in  the  x,  y ,  z 
direction 

Cross  product  between  wd3 
and  RA/G3  at  joint  2,  link 
3,  in  the  x,  y,  z  direction 

Cross  product  between  w3 
and  RA/G3  at  joint  2,  link 
3,  in  the  x,  y ,  z  direction 

MJC4,  MJC4  and  Cross 
product  between  w3  and 
MIB4,  MJB4,  and  MKB4 
respectively  at  joint  2. 
link  3,  at  the  x,  y,  z 
direction 

Used  in  LEQT2F  subroutine 
for  the  order  of  Mat A  and 
number  of  rows  of  vector  B 

Phase  angle  of  sine  wave 
input  to  joints 

Number  of  the  run 
conducting 

A  3x2  matrix  consisting  of 
the  distance  from  the 
center  of  gravity  of  the 
link  to  center  of  mass  for 
the  elements  of  link  1-3  in 
the  x  direction 

Same  as  Rx(3,2)  but  in  the 
y  direction 

Same  as  Rx(3,2)  but  in  the 
z  direction 
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COMPUTER 

SYMBOL 


TEXT 

VARIABLE 


DESCRIPTION 


RAG1(3) 

ra/Gl 

A  1x3  vector,  distance  of 
point  a  to  center  of 
gravity  for  link  1,  in  the 
x,  y,  z  direction 

RBG1(3) 

rb/Gl 

A  1x3  vector,  distance  of 
point  b  to  center  of 
gravity  for  link  1,  in  the 
x,  y,  z  direction 

RAG2 ( 3 ) 

ra/G2 

A  1x3  vector,  distance  of 
point  a  to  CoG  for  link  2, 
in  the  x,y,z  direction 

RBG2 ( 3 ) 

rb/G2 

A  1x3  vector,  distance  of 
point  b  to  CoG  for  link  2, 
in  the  x,  y,  z  direction 

RBG  3(3) 

rb/G3 

A  1x3  vector,  distance  of 
point  b  to  CoG  for  link  3, 
in  the  x,  y,  z  direction 

SUMHDX ( 3 ) 

EHDx 

A  1x3  vector,  sun  of  HDX 
for  the  two  elements  of 
link  1-3  in  the  x  direction 

SUMHDY ( 3 ) 

EHDy 

Same  as  EHDx  but  in  the  y 
direction 

SUMHDZ ( 3 ) 

EHDz 

Same  as  EHDx  but  in  the  z 
direction 

THETXR ( 3 ) 

A  1x3  vector  of  euler 
angles  in  the  x  direction 
in  radians  for  link  1-3 

THETYR( 3 ) 

Same  as  THETXR(3)  but  in 
the  y  direction 

THETZR( 3) 

Same  as  THETXR(3)  but  in 
the  z  direction 

TOX,  TOY 
TOZ 


Tox,  Toy 
Toz 


Input  torque  at  joint  0  at 
the  x,  y,  z  direction 


COMPUTER 

SYMBOL 

TEXT 

VARIABLE 

DESCRIPTION 

T1X,  T1Y 

T12 

Tlx,  Tly 

Tlz 

Input  torque  at  joint  1  at 
the  x,  y,  z  direction 

T2X,  T2Y 

T22 

T2x,  T2y 

T2z 

Input  torque  at  joint  2  at 
the  x,  y,  z  direction 

THDDOTO) 

Theoretical  value  of  wdx 
for  link  3  in  degrees 

THEORY ( 3 ) 

Theoretical  value  of  wdx 
for  link  3  in  radians 

THEXR1,  THEXR2, 
THEXR3 

Second  integral  of  wdx  for 
links  1-3  in  radians 

THEYR1,  THEYR2, 
THEYR3 

Second  integral  of  wdy  for 
links  1-3  in  radians 

THE2R1,  THE2R2, 
THE2R3 

Second  integral  of  wdz  for 
link  1-3  in  radians 

TORY1X 

Torylx 

Computed  value  of  torque 
for  joint  1 

TORY2X 

Tory2x 

Computed  value  of  torque 
for  joint  2 

TX1,  TX2,  TX3 

Euler  angle  theta  converted 
to  radians  for  links  1-3 
respectively  in  the  x 
direction 

TY1,  TY2,  TY3 

Euler  angles  theta 
converted  to  radians  for 
links  1-3  respectively  in 
the  y  direction 

T21,  TE2,  TZ3 

Euler  angles  theta 
converted  to  radians  for 
links  1-3  respectively  in 
the  z  direction 

VECTAO ( 3 )  and 
VECTBO ( 3 ) 

1x3  vector  used  in 

subroutine  CPROD  for  joint 

0 


COMPUTER 

SYMBOL 


TEXT 

VARIABLE 


DESCRIPTION 


VECTAK3)  and 
VECTBK3) 

1x3  vector  used  in 
subroutine  CPROD  for  joint 
1 

VECTA2 ( 3 )  and 
VECTB2 ( 3 ) 

1x3  vector  used  in 
subroutine  CPROD  for  joint 
2 

VECTA( 3 )  and 
VECTB( 3 ) 

1x3  vector  used  in 

subroutine  CPROD 

N 

w 

Frequency  of  sine  function 
input 

HI,  H2,  and 

H3 

HI, M2, M3 

Heights  of  link  1,  2,  and  3 

HX(  3 ) 

wx(  3 ) 

A  1x3  vector  of  angular 
velocity  of  link  1-3  in  the 
x  direction 

HY  ( 3 ) 

wy  ( 3 ) 

Sane  as  wx(3)  but  in  the  y 
direction 

HZ(  3 ) 

wz(  3) 

Sane  as  wx(3)  but  in  the  z 
direction 

HDX ( 3 ) 

wdz ( 3) 

Angular  acceleration  of 

link  1-3  in  the  x  direction 

HDY ( 3 ) 

vdy( 3) 

Angular  acceleration  of 

link  1-3  in  the  y  direction 

HDZ ( 3 ) 

wdz ( 3 ) 

Angular  acceleration  of 

link  1-3  in  the  z  direction 

HKAREA 

Hork  area  for  LEQT2F 

subroutine 

XI,  X2  and 

X3 

Location  of  center  of 
gravity  for  link  1-3  in  the 
x  direction 

Y 

y 

Theoretical  value  of  y 

distance  from  Fz2  to  center 
of  gravity  of  link  3 


COMPUTER 

SYMBOL 

TEXT 

VARIABLE 

DESCRIPTION 

Yl,  ¥2  and 
¥3 

Location  of  center  of 
gravity  for  link  1-3  in  the 
y  direction 

Z 

z 

Theoretical  value  of  z 
distance  froa  Fy2  to  center 
of  gravity  of  link  3 

Zl,  Z2  and 

Z3 

Location  of  center  of 
gravity  for  link  1-3  in  the 
z  direction 
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I.  INTRODUCTION 

Manipulator  models  which  use  local  coordinates  as  a 
basis  for  simulation  and  control  have  a  mathematical 
singularity  built  into  them  (Ref.  1).  This  singularity 
occurs  when  rigid  robot  links  align  such  that  their 
relative  position  is  either  0<>  or  180<> .  When  this  happens, 
the  inverse  of  the  Jacobian  matrix  becomes  impossible  to 
compute  and  the  forward  dynamics  solution  cannot  be  found. 

In  the  control  of  serial  link,  manipulators  there  have 
been  various  approaches  which  use  local  coordinates  to 
achieve  computational  efficiency.  One  method  deals  with  the 
Newton-Euler  approach  (Refs.  2,  3],  another  uses  the 
Lagrangian  approach  [Refs.  4,  5}  or  there  is  the  method  of 
virtual  work  [Ref.  6].  Still  another  that  has  tried  to  make 
the  solution  to  the  dynamic  equations  computationally 
efficient  by  using  Kane's  Dynamical  equations  [Ref.  7]. 
However,  although  these  methods  have  been  computationally 
efficient,  they  have  not  been  able  to  handle  the  problem  of 
singularity  (Ref.  1]. 
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Various  methods  have  been  proposed  to  deal  with  the 
problem  of  singularity.  One  such  method  is  to  minimize  the 
time  near  the  singularity  [Ref.  8],  thereby  reducing  its 
effects.  Another  solution  is  to  avoid  the  position  of  the 
manipulator  that  caused  the  singularity  [Refs.  9,  10]. 
However,  when  using  resolved  rate  equations  the  arm  may 
pass  through  a  point  of  singularity  anyway,  in  response  to 
a  command  (Ref.  11].  Nakamura  and  Hanafusa  [Ref.  12]  have 
proposed  to  determine  the  joint  motion  for  the  requested 
motion  of  the  end  effector  by  evaluating  the  feasibility  of 
the  joint  motion.  This  determined  joint  motion  is  called  an 
inverse  kinematic  solution  with  singularity  robustness. 
Other  solutions  deal  with  presenting  equations  that  can 
translate  the  manipulators  in  the  neighborhood  of 
singularity  [Refs. 13, 14]  and  in  identifying  geometric 
singular  positions  (Ref.  15]. 

It  has  also  been  shown  that  redundancy  of  robot 
manipulators  is  effective  in  dealing  with  singularities 
[Ref.  16].  Klein  and  Huang  [Ref.  17]  have  studied  the 
method  of  pseudo-inverse  control,  for  use  with  redundant 
manipulators,  with  recommendations  for  improvement. 
Uchiyama  (Ref.  18]  proposed  switching  the  control  mode  in 
the  neighborhood  of  singular  points  from  the  mode  using 
inverse  kinematics  to  the  joint  control  mode.  A  seven 
degree  of  freedom  kinematic  design  with  a  spherical 
shoulder  joint  was  proposed  [Refs.  19,  20),  as  well  as  a 


seven  joint  robot  [Ref.  21]  to  handle  singularity.  A  four 
degree  of  freedom  wrist  was  studied  to  overcome  wrist 
singularity  [Ref.  22].  Shih  [Ref.  23]  looked  at  the 
physical  quantities  and  combinations  of  physical  quantities 
which  are  unaffected  by  redundancy  to  simplify  the 
solution  of  a  redundant  system.  However,  even  though  there 
are  some  redundant  manipulators  constructed  [Refs.  24,  25], 
research  cannot  do  away  with  singularities,  and  so 
consideration  still  has  to  be  given  to  the  control  of  the 
manipulator  in  case  of  inadvertant  singularities. 

This  paper  will  derive  equations  of  motion  using  the 
First  Principles  of  Newtonian  dynamics  in  terms  of  global 
coordinates  in  order  to  eliminate  the  problem  of 
singularity.  By  the  method  of  free  body  analysis,  each  link 
of  the  manipulator  is  treated  as  if  it  were  a  free  body 
with  forces  and  moments  applied  at  the  joints.  Only 
revolute  joints  will  be  considered.  Although  tedious  and 
time  consuming  (computer  time),  this  paper  will  show  by 
simulation  how  the  problem  of  singularity  may  thus  be 
overcome . 
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This  thesis  does  not  deal  with  the  control  aspect  of 
rigid,  revolute  linkages  but  rather  the  mathematical 
dynaeic  modelling.  Given  the  dynaeic  model,  the  link  nesses 
and  inertia  properties,  initial  link  alignments,  and  joint 
torques,  then  the  joint  forces,  acceleration,  velocity  and 
position  can  be  predicted  via  a  simulation  program.  In  the 
present  approach,  all  dynamic  properties  except  for 
acceleration  and  forces  were  assumed  to  remain  constant 
over  a  simulated  time  interval.  This  assumption  linearized 
the  equation  of  motion  so  that  a  simple  matrix  inversion 
could  be  used  to  solve  for  the  unknowns.  As  shown  in  Figure 
1,  the  simulation  is  updated  with  the  predicted  velocity 

t 

(£.)  and  position  (§.),  following  integration  at  the  next 
time  step.  Simulation  validation  is  done  by  comparing  the 
theoretical  position  (thdy3)  to  the  predicted  position 
(8y3)  for  link  3  and  actual  torque  (Tlx,  T2x)  to  computed 
torque  (Torylx,  Tory2x)  for  links  2  and  3. 


III.  THEORETICAL  DEVELOPMENT 


A.  MANIPULATOR  ARM  CONFIGURATION 

This  thesis  develops  a  generalized  simulation  program 
for  a  robot  manipulator  that  is  a  serial  connection  of 
three  rigid  links,  jointed  by  one-degree  of  freedom 
revolute  joints.  Joint  actuators  are  assumed  to  be  located 
between  successive  links  to  apply  the  torque  necessary  to 
position  the  link. 

B .  THEORY 

The  method  of  solution  is  based  on  the  principle  of 
free  body  analysis.  For  this  approach  each  body  of  the 
three  link  manipulator  is  treated  as  if  it  were  a  free  body 
with  forces  and  moments  applied  at  each  joint,  as  shown  in 
Figure  2.  The  global  cartesian  coordinate  system  X,  Y,  Z  as 
well  as  force  and  moment  torque  conventions  are  also 
evident  in  the  figure.  Note  that  a  local  coordinate  system, 
that  is  a  coordinate  system  that  is  local  to  each  joint, 
will  not  be  used  but  rather  a  single  global  system  will  be 
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adopted.  So  all  positions,  distances,  etc.,  will  be 
referenced  to  the  base  of  the  manipulator  system  which  will 
be  at  joint  zero.  The  effects  of  flexibility  of  the  robot 
manipulator  will  not  be  considered  since  ideal,  rigid 
bodies  are  assumed. 

In  developing  the  dynamic  equations  of  motion  for  each 
link  Newton's  Second  Principle  of  Motion  is  used.  The  known 
variables  are  torque  at  the  joints,  mass  of  each  link, 
linear  acceleration  of  joint  zero,  initial  angular 
acceleration,  angular  velocity  and  position  of  all  links. 
The  unknowns  are  the  forces  at  the  joints,  linear  and 
subsequent  angular  accelerations  of  the  links. 

C.  DYNAMIC  EQUATIONS  OF  MOTION  OF  LINK  ONE 
1-  Sum  of  Forces  Equations 

In  the  free  body  analysis  of  link  one  (Figure  2)  the 
sum  of  the  forces  in  the  x  direction  is: 

EFx=Fxl  -  FxO=  Mlaxl  (1) 

Similarly  sum  of  the  forces  in  the  y  direction  is: 

CFy»Fyl  -  FyO«Mlayl  (2) 

and  tbm  sum  of  the  forces  in  the  z  direction  is: 


EFz«Fzl  -  FzO  -  Wl=Mlazl 


(3) 


A  '■'^  A  ^  W.*'  W  »■  V*  «7  «.■"!*  -.» -T-TT  *V  *7 v.  '-"  v. 


2.  Joint  Equations 

He  begin  by  evaulating  the  joint  equations  at  joint 
zero  (Ref.  26,  equation  (8/4),  pp.  423).  If  the  joint  is 
sequested  and  analysis  conducted  at  a  point  on  link  zero 
(subscript  a)  and  another  at  a  point  on  link  one  (subscript 
b)  that  is  conon  to  both,  so  when  linked  together  they  are 
equal.  This  results  in  two  equations  and  the  two  unknowns 

wdl  and  wl. 

As  a  result: 

*3  -  *9 

which  is  the  acceleration  at  joint  zero, 
and 

Ab  *  A1  +( wdl  X  rb/Gl)  ♦  wl  X  (wl  X  rb/Gl) 
which  is  the  acceleration  of  point  b  on  joint  one.  Here 
rb/Gl  is  the  distance  from  point  b  to  the  center  of  gravity 
of  link  one,  and  A1  is  the  acceleration  at  the  center  of 
mass  of  link  one  or, 

rb/Gl* ( jxO-LCOGxl ) i  +  ( jyO-LCOGyl ) j  +  ( j zO-LCOGzl )k 
*rb/Glx  +  rb/Gly  +  rb/Glz 

After  equating  Aa  and  Ab  and  having  the  known  variables  on 
the  right  side  of  the  equation  and  unknown  variables  on  the 
left  side  the  following  sets  of  equations  result: 

Axl  +  wdyl ( rb/Glz ) -wdzl ( rb/Gly ) ■  Aox-MICO  (4) 

where  MICO  equals 
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=wylwxl(rb/Gly )-w*yl(rb/Glx)-wlzl (rb/Glx) 

4-  vzlwxl(rb/Glz) 

also 

Ayl  4-wdzl(rb/Glx)-wdxl(  rb/Glz  )=Aoy-MJCO  (5) 

where  HJCO  equals 

=wz lwy 1( rb/Glz )-w*x(rb/Gly )-w*xl(rb/Gly ) 

4-  wxlwyKrb/Glx) 

and 

Azl  wdxl(rb/Gly)-wdyl(rb/Glx)=Aoz-MKCO  (6) 

NKCO  equals 

swxlwzl ( rb/Glx ) -w*xl ( rb/Glz ) -w*y  1 ( rb/Glz ) 

4-  wylwzl(rb/Gly ) 

3.  Sam  oi  Hoagat .EauaUgna 

Computing  the  sue  of  the  eoeent  equations  about  the 
center  of  gravity  results  in: 

£Ml=»(rO/Gl  X  FO)  4  (rl/Gl  X  F1)-T1  4-  TO 

'V  ^  ^  ^  -w 

where  the  vector  rO/Gl  is  the  distance  from  joint  zero  to 
the  center  of  gravity  of  link  one  and  vector  rL/Gl  is  the 
distance  froe  joint  one  to  the  center  of  gravity  of  link 
one,  in  the  x,  y  and  z  directions.  Such  that 
rO/Gl  *  rjo-rGl 

and 

rl/Gl  -  rjl-rGl 

80 

rjO-rGl»(xjO-xGl)i  4-  (yjO-yGl)j  4-  (zjO-zGl)k 

and 
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rjj.  -rjG  1  =  (xjl-xGl)  i  +  (yjl-yGl)j  +  zjl-zGl)k 


In  the  x ,  y  and  z  directions  the  sue  of  moment  equations 
are : 

EMI  in  x  direction* 

(-yjO/Gl)FzO  +  (zjO/Gl )FyO  +  (yj 1/GI )Fzl-( zj 1/G1 )Fyl 
-Tlx  ♦  TOx  (7a) 

EMI  in  y  direction* 

(-zjO/Gl)FxO  +  (xjO/Gl)FzO  +  (zjl/Gl)Fxl-  (xjl/Gl)Fzl 
-Tly  +  TOy  (8a) 

EMI  in  z  direction* 

(-xjO/Gl)FyO  +  (yjO/Gl)FxO  +  (xjl/Gl)Fyl-(yjl/Gl)Fxl-Tlz 
+  TOz  (9a) 

From  Ref.  26,  equation  (517)  pp.227  the  sum  of  the  moments 
about  a  fixed  point  that  does  not  move  with  the  body  is 
equal  to  the  time  rate  of  change  of  angular  momentum  of  the 

a  • 

system  (H)  about  the  fixed  point,  EM*H.  In  the  present 
study  we  have  let  each  link  be  a  composite  body  of  two 
elements.  The  angular  momentum  (H)  for  a  composite  body 
where  the  number  of  elements  of  the  body  is  two,  about  the 

center  of  gravity  of  each  link  is  Hi*^  (Ri  X  (w  X  Ri)]Mi, 

<:•» 

where  Ri  is  the  distance  from  the  center  of  gravity  of  each 
link  to  the  appropriate  element  (lor2)  in  the  x,  y  and  z 
direction.  So: 

Hx  *  E*  ( Ryi ( wx( Ryi ) -wy ( Rxi ) ) -Rzi ( wz(Rxi ) wx-(Rzi ) ) ]Mi 
Hx  ■( R* y 1 ( wx ) -Ry 1( Rxl ) ( wy ) -Rz( Rxl ) ( wz )  +  Rzl(wx)]Ml 

♦  { R*y2(  wx )  -Ry2( Rx2 )  ( wy )  -Rz2 ( Rx2  )  ( wz  )  +  (R*’z2)wx]M2 

">0 


a  -• 


.  s'.  s.  A 


If  I XX 


2  +Rza  dm. 


JRy 
JRxRy  dm. 


and  Ixy 
and 

Ixz  = JRxRy  dm. 


then: 

Hx  =  [Ilxx(wx)  -  Ilxy(wy)  -  Ilxz(wz))Ml 

+  [ I2xx( wx)  -  I 2xy ( wy )  -  I2xz(wz)JM2. 

and 

HDx  ■  [Ilxx(wdx)  -  Ilxy(wdy)  -  Ilxz(wdz)]Ml 

+  ( I 2xx ( wdx )  -  I 2xy ( wdy )  -  I2xz(*dz) ]H2  (7b) 

by  assuming  the  moment  of  inertia  does  not  change  with  time 
but  is  constant  for  a  given  time  interval. 


By  similar  analysis  it  can  be  shown: 

« 

E 

4*1 

and  if  Iyy* 


Hy-  E  (Rzi(wy(Rzi)-wz(Ryi) ) -Rxi( wx(Ryi) -wy(Rxi) ) ]Mi 


'Rx2  +  Rz*  dm. 


and 

and 


Iyz= 


RyRz  dm. 


Ixy*  JRxRy  dm 


then: 

HDy*lIlyy(wdy )-Iiyz(wdz) -Iiyx( wdx) JMi 
+  ( I 2yy ( wdy ) -I 2yz ( wdz ) -I 2yx ( wdx ) ) M2 

and 


(8b) 


Hz*E'  (Rxi(wz(Rxi)-wx(Rzi) ) -Ryi ( wy ( Rzi ) -wz( Ryi ) ) JMi. 
If  Izz=  JRx  tRy*'  dm. 


So 
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Hz=[ I lzz ( wz ) -I lyz ( wy ) -I Izx ( wx ) ]M1 

♦  I12zz(wz)-I2yz(wy)-I2zx(wx) 1M2 

then 

HDzs( I lzz ( wdz ) -I lyz ( wdy ) -I lzx ( wdx ) ] Ml 

+  (I2zz(wdz)-I2yz(wdy) -I2zx(wdx) JM2  (9b) 

Combining  equations  (7a)  and  (7b)  and  keeping  known 
variables  on  the  right  side  and  unknown  variables  on  the 
left  side  yields: 

EMlx«  ( -yjo/Gl  )Fzo  +  (zjo/Gl)Fyo  (yjl/Gl)Fzl 

-  ( z j 1/G1 ) Fyl-HDx*Tlx-Tox  (7) 

Combining  equations  (8a)  and  (8b)  yields: 

EMlya(  -z  jo/Gl  )Fxo  +  (xjo/Gl)Fzo  +  (zjl/Gl)Fxl 

-  (xjl/Gl )Fzl-HDy=Tly-Toy  (8) 

combining  equations  (9a)  and  (9b)  yields: 

EMlz=*-(  x  jo/Gl  )Fyo  +  (yjo/Gl)Fxo  +  (xjl/Gl)Fyl 

-  (yjl/Gl)Fxl-HDz-Tlz-Toz  (9) 


0.  LINK  TWO  EQUATIONS 

1.  Sum  of  Forces  Equations 

From  the  free  body  diagram  (Figure  2)  it  follows 

that 


EFx-Fz2 

-  Fxl-H2ax2 

(10) 

EFy«Fy2 

-  Fyl-M2ay2 

(ID 

EFz=Fx2 

-  Fzl=H2az2 

(12) 

2 .  Joint  Equations 


Analysis  is  conducted  at  joint  one  where  similar 
equations  are  used  as  in  joint  zero  with  a  point  on  link 
one  (a)  and  one  on  link  two  (b).  For  point  a  the  equation 
is 

Aa*  A1+  wdl  X  ra/Gl  +  wl  X  (wl  X  ra/Gl) 

^  ^  /V*  A/ 

ra/Gl  is  a  vector  whose  distance  is  Measured  from  point  a 

A/ 

to  the  center  of  gravity  of  link  one  in  the  x,  y  and  z 
direction. 

ra/Gl=( jxl-LCOGxl)i  +  ( j yl-LCOGyl ) j  +  ( jzl-LCOGzl )k 
*ra/Glx  +  ra/Gly  +  ra/Glz 
For  point  b  the  equation  is: 

Ab=»A  2  +  wd2  X  rb/G2  +  w2  X  (w2  X  rb/G2) 

AW  /V  -AW  /V 

where  rb/G2  is  a  vector  whose  distance  is  measured  from 
point  b  to  the  center  of  gravity  of  link  two. 

rb/G2=< jxl-LC0Gx2)i  +  ( jyl-LC0Gy2 ) j  +  ( j zl-LC0Gz2 ) k 
=rb/G2x  ♦  rb/G2y  rb/G2z 

Equating  Aa  and  Ab  and  setting  knowns  and  unknowns  on 
the  respective  sides  of  the  equation  results  in: 

Ax2-Axl  +  wdy2 ( rb/G2z ) -wdz2 ( rb/G2y ) -wdyl ( ra/Glz )  + 
wdzl ( ra/Gly) "MIC1-MIC2  (13) 

MICl«wylwxl (ra/Gly ) -w2yl ( ra/Glx ) -w2zl ( ra/Glx ) 

♦  wzlwxl ( ra/Glz ) 

MIC2=wy2wx2 (ra/G2y ) -w2y2 ( ra/G2x ) -w2z2 ( rb/G2x ) 

+  wz2wx2 ( rb/G2z ) 
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Ay2-Ayl  +•  wdz2  ( rb/G2x )  -wdx2  (rb/G2z ) -wdzl  ( ra/Glx) 

♦  wdxl  ( ra/Glz  )  ■MJC1-MJC2 
MJCl*wzlwyl(ra/Glz) -w2zl(ra/Gly ) -w2xl ( ra/Gly ) 

+  wxlwyl ( ra/Glx ) 

MJC2«wz2wy2 ( rb/G2z ) -w2z2 ( rb/G2y ) -w2x2 ( rb/G2y ) 

♦  wx2wy2( rb/G2x ) 

AZ2-AZ1  +  wdx2( rb/G2y )  -wdy2 ( rb/G2x )  -wdxl  ( ra/Gly ) 


♦  wdy  1  ( ra/Glx ) =  HKC1-MKC2 
MKCl*wxlwzl( ra/Glx )-w2xl(ra/Glz)-w2yl(ra/Glz) 

♦  a ylvzl(ra/Gly) 

MKC2-wx2wz2 ( rb/G2x ) -w2x2(rb/G2z ) -w2y2 (rb/G2z ) 

♦  wy2wz2 ( rb/G2y ) 

3.  Sam,  o*  the  Hoaent  Equations 

Thasa  aquations  hava  a  siallar  developaent 
of  link  one: 

EM2  *(r  j 1/G2)  X  PI  +  ( r j  2/G2 )  X  F2  +  T1-T2 

^  /\y  -v  'W  'V'  ■%/ 


where 


r  j  1/G2~( xj  l-xG2 )  i  ♦  <yjl-yG2)j  ♦ 
r  j  2 /G2*( x j  2-xG2 )  i  ♦  (yj2-yG2)j  ♦ 
EM2x=  -( y j l-yG2 )Fzl  +  (zjl-zG2)Fyl  + 

-  ( x j  2-zG2 ) Fy  2  +  Tlx-T2x 

EH2y«  -(xjl-zG2)Fxl  ♦  (xjl-xG2)Fzl  + 

-  ( x j  2-xG2 ) Fz2  ♦  Tly-T2y 
EM2z*-  ( xj  l-xG2  )Fyl  ♦  (yjl-yG2)Fxl  + 

-  ( y j  2-yG2 ) Fx2  +  Tlz-T2z 


(zjl-zG2)k 
(  z j  2-zG2 ) k 
<yj2-yG2)Fz2 

( z j  2-zG2 )Fx2 

(xj2-xG2)Fy2 


( 14) 

( 15) 

as  that 


( 16a) 

( 17a) 

(18a) 


Froa  angular  aoaentua  equation  developed  for  link  one,  it 


can  be  shown  for  1  ink  two : 


2M2x*HDx 

(16b) 

£M2y»HDy 

(17b) 

EM2z«HDz 

(18b) 

Combining  equations  (16a)  and  (16b)  the  following  result: 

-(yjl-yG2)Fzl  +  ( z j l-zG2 )Fyl  +  (yj2-yG2)Fz2-(zj2-zG2)Fy2 
-HDx— Tlx  +  T2x  (16) 

Combining  equations  (17a>  and  (17b)  yield  the  following 
result : 

-( z j l-zG2 )Fxl  ♦  (xjl-xG2)Fzl  +  (zj2-zG2)Fx2-(xj2-xG2)Fz2 
-HDy—Tly  ♦  T2y  (17) 

Combining  equations  (18a)  and  (18b)  yield  the  following 
result: 

-(xj l-xG2)Fyl  +  (yjl-yG2)Fxl  +  (xj2-xG2)Fy2-(y}2-yG2)Fx2 
-HDz»-Tlz  ♦  T2z  (18) 


E.  LINK  THREE  EQUATIONS 

1.  Sum  of  Forces  Equations 

Following  similar  logic  from  that  previously 
developed: 

EFx«  -Fx2«M3ax3  (19) 

EFy-  -Fy2-M3ay3  (20) 

£Fz«  -Fz2  -  N3«M3az3  (21) 

2.  Joint  Equations 

Hith  point  a  on  link  two  and  point  b  on  link  three 
one  gets  for  joint  equations  at  joint  two: 


Aa-A2  +  ( vd2  X  ra/G2)  +  i»2  X  (w2  X  ra/G2) 

^  a/  Ay  <v  ^ 

where  ra/G2  is  a  vector  whose  distance  is  Measured  from 

/N/ 

point  a  to  center  of  gravity  of  link  two  in  the  x,y  and  z 
direction. 

ra/G2«( jx2-LCOGx2)i  +  ( jy2-LCOGy2 ) j  +  ( jz2-LCOGz2)k 
»ra/G2x  4-  ra/G2y  +  ra/G2z 
For  point  b 

Ab=A3  -l-  wd3  X  rb/G3  +  w3  X  (w3  X  rb/G3) 

A/  -V  A/  A*  A*  ^ 

where  rb/G3  is  a  vector  whose  distance  is  Measured  from 

A— 

point  b  to  center  of  gravity  of  link  three  in  the  x,  y  and 
z  direction. 

rb/G3*»(  jx2-LCOGx3 )  i  +  (  jy2-LCOGy3 )  j  +  ( jz2-LCOGz3)k 
»rb/G3x  4-  rb/G3y  +  rb/G3z 

Equating  Aa  and  Ab  and  setting  knowns  and  unknowns  on  the 
respective  sides  of  the  equation  results  in: 

Ax3-Ax2  +  wdy3(rb/G3z)-wdz3(rb/G3y )-wdy2(ra/G2z) 

♦  wdz2(ra/G2y)»MIC3-MICl  (22) 

MIC3=wy2wx2(ra/G2y )-w2y2(ra/G2x)-w2z2(ra/G2x) 

+  wz2wx2(ra/G2z) 

MIC4«  wy3wx3(rb/G3y ) -w2y3( rb/G3x ) -w2z3(rb/G3x) 

4-  wx3wx3  (  rb/G3z ) 

Ay3  -Ay 2  +  wdz3(rb/G3x)-wdx3(rb/G3z)-wdz2(ra/G2x) 

+wdx2( ra/G2z ) »MJC3-MJC4  (23) 

MJC3»wz2wy2(ra/G2z)-w2z2(ra/G2y )-w2x2(ra/G2y ) 


wx2wy2(  ra/G2x) 


MJC4=wz3wy3 ( rb/G3z ) -w2z3 ( rb/G3y ) -w2x3 ( rb/G3y ) 


♦  wx3wy3 ( rb/G3x ) 

AZ3-AZ2  +  wdx3(rb/G3y)-wdy3(rb/G3x)-wdx2(ra/G2y) 

+  wdy2 ( ra/G2x ) =  MKC3 -MKC4  (24) 

MKC3=«x2wz2(ra/G2x)-w2x2(ra/G2z ) -w2y2(ra/G2z ) 

♦  wy2wy2 ( ra/G2y ) 

MKC4=wx3wz3(rb/G3x)-w2x3(rb/G3z ) -w2y3 ( rb/G3z ) 

♦  wy 3wz3 ( rb/G3y ) 

3. Saw  of  Mowent  Eauationa 

As  in  the  developMnt  of  the  equations  for  link,  one: 
EM3=(rj2/G3)  X  F2  +  T2 

***»  A/  A/ 

where  rJ2/G3«*(xj2-xG3)i  f  (yj2-yG3)j  +  (zj2-zG3)k 


*xj  2/G3  +  y j  2/G3  ♦  zj2/G3 

£M3x*( -y j  2/G3 )Fz2  +  (zj2/G3)Fy2  +  T2x  (25a) 

EM3y-  ( -z j  2/G3 ) Fx2  +  (xj2/G3)Fz2  +  T2y  (26a) 

EM3z*(-xj2/G3)Fy2  +  (yj2/G3)Fx2  +  T2z  (27a) 

From  the  angular  soaentus  theory: 

EM3x«HDx  (25b) 

EH3y»H0y  (26b) 

EM3z*H0z  (27b) 

Combining  equations  (25a)  and  (25b)  the  following  results 
(-yJ2/G3)Fz2  +  ( z j 2/G3 )Fy2-HDx=  -T2x  (25) 

Combining  equations  (26a)  and  (26b)  the  following  results 
(  ( -z j  2/G3 )Fx2  +( xj  2/G3 )Fz2-HDy=  -T2y  (26) 

Combining  equations  (27a)  and  (27b)  the  following  results: 
( -x j  2/G3 ) Fy2  +  ( y j 2/G3 ) Fx2-HDz=  -T2z  (27) 
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The  language  chosen  to  write  the  prograa  was  the 
Digital  Siaulation  Language  (DSL)  using  Fortran  77  coding. 
This  language  does  an  excellent  dynamic  siaulation  that 


allows  the  user  to  be  interactive,  with  real  tiae 
processing  vice  batch  aode  processing  coaaonly  used  with 
the  Continuous  Systea  Modelling  Prograa  (CSMP),  and  all 
calculations  done  in  double  precision.  The  source  code 
produced  Cor  this  prograa  was  coaplied  on  an  IBM  3033 
coaputer  using  a  Fortran  77  coapiler. 

A.  PRINCIPLE  PROGRAM  MATRICIES 

A  27x27  Matrix  A  (MatA)  was  created  from  the 
coefficients  of  the  unknowns  (forces,  linear  acceleration 
and  angular  acceleration)  froa  equations  (1)  to  (27). 
Correspondingly  a  27x1  Matrix  B  (MatB)  was  generated  froa 
equations  (1)  to  (27)  froa  all  knowns  (torques,  angular 
velocities,  link  aasses,  and  various  positions).  Subroutine 
CPROD  was  used  to  conduct  all  cross  products  required  in 
the  aaln  prograa.  Subroutine  (LEQT2F)  was  then  called  from 
the  IMSL  library.  This  subroutine  takes  MatA  inverts  it, 
multiplies  it  by  MatB  and  solves  the  generalized  equation 
A$=b  for  the  b  vector  using  Gaussian  elimination  with 


iterative  improvement  to  get  accuracy  within  six  decimal 
digits.  The  output  from  LEQT2F  returns  from  the  subroutine 
via  MatB.  This  output  is  then  used  by  the  IHTGRL  DSL 
statement  to  take  the  integral  of  angular  acceleration 
(wdx,  wdy,  wdz)  to  get  angular  velocity  (vx,  wy,  wz)  and 
again  to  get  the  position  of  the  link  with  respect  to  theta 
(c0x,  c9y,  c6z )  for  each  torque  input  per  time  step.  The 
cartesian  orientations  are  converted  to  Euler  angles  (0x, 
ay,  6z)  prior  to  returning  to  the  beginning  of  the  program. 
The  method  used  to  solve  the  second  order  differential, 
equation  for  accelerations  is  invoked  by  ADAMS  which  is  the 
second  order,  variable  step  integration  ADAMS  method.  This 
method  was  shown  to  be  the  fastest  (CPU  time)  and  most 
accurate  of  the  methods  available  [Ref.  81.  Similarly, 
INTGRL  is  applied  to  find  the  linear  acceleration  (A)  of 
each  link,  velocity  (V_)  and  finally  the  position  of  the 
center  of  gravity  of  the  link.  These  newly  found  values  are 
fed  back  into  the  beginning  of  the  simulation  program  for 
the  next  time  step  until  the  end  of  the  interval.  This 
process  is  summarized  in  Figure  3. 
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Constraints  are  also  built  into  the  simulation  program 
that  enable  the  operator  to  limit  the  movement  of  the  links 
in  the  yz  plane  for  a  two  dimensional  demonstration  of 
link -three-only  movement.  The  constraints  are  also  used  for 
link  two  and  three  movement.  The  constraints  are  applied  by 
zeroing  out  a  row,  except  for  the  diagonal  which  is  set  to 
1.0.  The  MatB  entry  is  set  to  the  constrained  value. 

It  is  during  this  simulation  that  a  differentiation 
should  be  made  between  the  cartesian  theta  (c0)  position 
developed  by  the  INTGRL  function  (Figure  4a)  and  the  Euler 
angler  theta  (6)  used  as  direction  angles  in  computing 
distances  (Figure  4b).  When  in  the  yz  plane  the  angular 
acceleration  is  about  the  x  axis  and  when  the  integral  is 
taken  twice  with  respect  to  time,  what  results  is  the  angle 
theta  about  the  x  axis.  This  cartesian  angle  is  defined  as 
cOx  and  is  obviously  not  the  same  as  the  theta  angle  used 
to  position  the  link  initially  which  is  defined  as  6y.  To 
resolve  this  discrepancy  when  c8x  is  computed  it  is 
converted  to  the  euler  angle  dy  by  setting  the  two  equal  so 
0y=c0x,  in  a  two-dimensional  simulation.  Additionally, 
euler  angle  0z=9O°-0y  and  0x»9O<>  whenever  simulating,  two- 
dimensional  yz  plane  motions. 


B.  PROGRAM  VALIDATION 


Validation  of  the  simulation  program  takes  place  in  two 
ways . 

1.  Validation  of  One  Link  Case 

For  link  three  the  theoretical  value  of  theta  in  the 
z  direction  (th6x3),  is  compared  to  the  value  of  theta  in 
the  z  direction  (8x3)  that  the  simulation  computed  for  each 
time  step  (Appendix  A  has  the  program  listing). 

As  a  test  case/ the  torque  delivered  at  joint  two  was 
assumed  to  be: 

T2x-10sin(2*t) . 

Also,  Ixxa,  the  moment  of  inertia  about  joint  two,  is  equal 
to  mass  at  the  end  of  the  link  M(3,2)  times  the  distance 
from  joint  two  to  the  mass  at  the  end  of  the  link,  squared 
or, 

* 

Ixxa=H ( 3,2)  x  (L(3,2)  +  L(3,l))  . 

This  may  be  used  to  solve  for  thdx  by  taking  the  integral 
with  respect  to  the  time  which  results  in: 

th6x3=»T2x 

Ixxa 

'*  l* 

ft  th0x3  dt»-10  cos  (2*t) 

J0  Ixxa(2x) 

the»3»( -10  (cos2xt)  +  lfl )  1 _ 

2*  2x  Ixxa 

fth6z3  dt«  th8x3°(-10  sin(  2xt )  +  lQt  )  1 _  +  s. 

J  4***2  2*  Ixxa  4 
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For  comparison  %  error  is  used  so 


%  error ■( th03-0x3)  x  100 
max  th0x3 

2.  Validation  of  Two  Links  Case 

For  the  validation  of  two  links  the  computed  torques 
at  joints  two  and  joint  one  (Tory2x,  Torylx)  are  compared 
to  the  torques  that  are  actually  input  (T2x,  Tlx)  at  each 
time  step  (Appendix  B  has  the  program  listing).  If  there 
are  no  effects  of  singularity  then  the  theoretical  torque 
and  input  torque  should  be  very  similar.  From  Figure  2  the 
sum  of  the  moments  about  the  center  of  gravity  of  link 
three  is: 

EM3-M3(L( 3,2) 2 ( wdx ( 3 ) ) =T2x  +  Fz2y  -  Fy2z 
so 

T2x=M3(L(3,2))2(wdx(3))  -  Fz2y  +  Fy2z=Tory2x 
where 

y=L( 3,1) (cos(0y3) ) 

Fy2=(-M3)(ay3) 
z=L(3,l) (cos(6y3) ) 

Fz2=s(  -M3)  (az3) 

Sum  of  the  moments  about  the  center  of  gravity  of  link  two 
is : 

EM2«Ixx2( wdx( 2) )=Tlx-T2x  +  Fzlcos ( 0y2 ) ( L( 2, 1 ) ) 
-Fylsin(0y2) (L( 2, 1) )  +  Fz2cos ( 0y2 ) ( L( 2,  2 ) ) 

-Fy2sin(0y2) (L( 2,  2) ) 


Tlx=M2(L(2, 2) )2( wdx( 2) )  +  T2x-Fzlcos (6y2 ) (L< 2, 1 ) ) 

+  Fylsin(dy2 ) (L( 2, 1) ) -Fz2 (L( 2,2) )cos (9y2 ) 

+  Fy2sin(0y2) (L( 2, 2 ) ) -Tory lx 

where 

Fzl=Fz2-M2(az2) 

Fyl*Fy2-M2ay2 . 

For  comparison  the  %  error  is  used  for  difference  in 
torque  for  joint  two  and  joint  one: 

Errt2x=(Tory2x-T2x)/(eaxTory2x)  x  100 
Errtlx«(Torylx-Tlx)/(aaxTorylx)  x  100 


V.  RESULTS 


A.  MOVEMENT  OF  LINK  THREE 

Analysis  of  the  movement  of  only  link  three  shows  very 
good  results  for  program  validation.  Figure  5  shows  a  plot 
of  Euler  angles  for  both  theoretical  (they 3)  and  simulated 
(6y3)  values,  the  graph  shows  indistinguishable 
differences.  To  further  visualize  the  difference  Figure  6 
was  plotted,  which  is  the  %  error  between  they  and  8y 
versus  time.  There  seems  to  be  greater  error  (0.0032%)  at 
around  0.8  seconds  than  at  0 . 2  seconds.  This  could  possibly 
be  caused  by  error  buildup  in  the  computation  due  to  round 
off  error  from  subroutine  LEQT2F  and  truncation  error  from 
approximating  the  solution  to  the  second  order  differential 
equation  by  the  ADAMS  method.  Additionally,  inaccuracies 
could  occur  in  estimating  the  value  of  *  and  using  it  in 
trigonometric  calculations . However , the  %  error  is  small  and 
is  acceptable  to  verify  the  proper  operation  of  the  program 
for  the  single  degree  of  freedom  case. 


Theoretical  and  Simulated 


B.  MOVEMENT  OF  LINK  TWO  AND  THREE 

Analysis  of  the  movement  of  link  two  and  three  is  the 
crucial  test  of  how  the  simulation  deals  with  the  problem 
of  singularity.  A  torque  was  input  to  joint  two  and  an 
opposite  torque  to  joint  one.  At  some  point  the  alignments 


of  the 

two 

links  will 

have 

some 

absolute  angle  of 

0O 

(Figure 

7) 

relative  to 

each 

other . 

At 

this  time 

if 

singularity 

exists  there 

is  no 

longer 

any 

control  of 

the 

links  and  accelerations  and  velocities  vary  abnormally, 
never  returning  to  the  level  they  were  at  before  the 
singular  position  was  reached  [Ref.  8).  So  the  reason  for 
comparing  the  values  of  the  computed  torques  (Tory2x, 
Torylx)  given  the  position  variables  solved  for  by  the 
simulation  program  and  the  torques  input  to  the  joints 
(T2x,  Tlx),  is  to  check  for  abnormalities.  Figure  8  shows 
the  graph  of  computed  and  input  torques  for  joint  two  and 
Figure  9  shows  the  graph  of  computed  and  input  torques  for 
joint  one  versus  time.  The  two  curves  match  very  well  and 
shows  almost  no  deviation  between  them  for  the  scale  used. 

When  the  %  errors  are  plotted  between  computed  torque 
and  input  torque  versus  time  for  links  two  and  one  (Figures 
10  and  11)  again  very  little  %  error  is  observed  with  the 
largest  being  around  0.024%  at  time  2.8  seconds  for  torque 
input  at  joint  one.  This  may  be  attributed  to  the  similar 
reasons  as  the  one-link  since  now  both  link  two  and  three 


are  moving  these  errors  are  building  as  time  increases.  It 
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VI.  CONCLUSIONS 


The  ability  of  a  global  two  degree  of  freedom  robot  are 
to  maneuver  through  a  point  of  singularity  under  applied 
torques  was  demonstrated.  This  was  verified  by  comparing 
the  computed  torque  at  joint  one  and  two  in  the  x  direction 
to  the  values  that  were  input.  There  were  no  unusual  or 
abnormal  results  occurring  in  the  acceleration  or 
velocities  and  so  little  error  was  produced. 


r 
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The  following  recommendations  are  provided: 

1.  Develop  a  linearized  manipulator  modal  and  a 
corresponding  controller  for  the  two  degree  of  freedom 
case . 

2.  Validate  the  approach  via  actual  empirical  tests  for  the 
two  dimensional  case.  This  will  establish  the  difficulty  of 
determining  accurate  constants  for  the  simulation  and 
controller  design. 

3.  Demonstrate  the  model  and  controller  in  3  dimensions 
with  3  links.  The  difficulty  here  arises  in  analyzing  the 
direction  of  a  given  joint  torque  in  3  dimensions.  This 
could  probably  be  done  by  finding  a  unit  normal  vector 
perpendicular  to  the  joint  in  the  x ,  y  and  z  direction  and 
multiplying  it  by  the  torque  magnitude. 

4.  Validate  the  approach  by  implementation  for  the  three 
dimensional  case. 


APPENDIX  A 

SIMULATION  PROGRAM  FOR  MOVEMENT  OF  LINK  THREE 


TERMINAL 
METHOD  ADAMS 

PRINT  .01, ETHETY ( 3 ) ( EULORY ( 3 ) ,ERR0R(3) 

CONTROL  FINTIM  =1.0,  DELMAX  =.01,  DELPRT  =  .01 
SAVE  .01 ,ETHETY(3) , EULORY (3) ,ERR0R(3) 

GRAPH(DE=TEK618)  TIME , ETHETY ( 3 ) 

GRAPH(DE=TEK618)  TIME , ERROR (3 ) 

GRAPH (DE=TEK6 18)  TIME ,EUL0RY(3) 

D  DIMENSION  MATA (27 . 27 ) , MASS (3 ,2) , L(3 , 2) ,RX(3,2) ,RY(3,2) ,RZ(3,2) 

D  DIMENSION  IXX(3 , 2 ) , IXZ(3 , 2 ) , IXY(3 , 2) , IYY(3 , 2 ) , IYZ(3 , 2 ) , IZZ(3 , 2 ) 

D  INTEGER  IER , I , RUN , M , N , IA , IDGT 
EXCLUDE  IA , IDGT , IER , I , RUN , M , N 

ARRAY  MATB(27 ) ,LCOGX( 3 ) , LCOGY( 3 ) , LCOGZ (3 ) , ETHETX(3) , ETHETY ( 3 ) , ETHETZ ( 3 ) 
ARRAY  CTHETX ( 3 ) , CTHETY ( 3 ) , CTHETZ  3 ) , THDDOT(3) , IXXA(3) , ERROR? 3) 

ARRAY  VECTA0 ( 3 ) , VECTBO ( 3 ) , VECTA1 ( 3 ) , VECTB1 ( 3 ) , VECTA2 ( 3 ) , VECTB2 ( 3 ) 

ARRAY  WDX(3 ) ,WDY(3) ,WDZ(3) , WX(3 ) , WY(3 ) . WZ(3 ) , RBG1 ( 3 ) , RAG1 (3 ) , THEORY( 3 ) 
ARRAY  RBG2(3) ,RAG2(3) , RBG3 ( 3 ) , THETXR ( 3 ) , THETYR ( 3 ) , THETZR ( 3 ) , EULORY ( 3 ) 
ARRAY  HDX ( 2 ) , HDY ( 2 ) , HDZ ( 2 ) 

ARRAY  SUMHDX(3 ) ,SUMHDY(3) ,SUMHDZ(3) ,WKAREA(850) 

D  DATA  MATA/729  *  0./ 

INITIAL 

*  INPUT  PARAMETER  CONSTANTS 

A  =  10.0 
P  =  0.0 
W  =  2*Pr 
IDGT  =  4 
G=0 . 0 
N=27 
M=1 

IA  =27 
RUN  =  1 

*  INPUT  JOINT  LOCATIONS  IN  METERS 

JXO  =  0.0 
JYO  =  0.0 
JZO  =  0.0 
JX1  =  0.0 
JY1  =  1.0 
JZ1  =  0.0 
JX2  =  0.0 
JY2  =  2.0 
JZ2  =  0.0 

*  INPUT  TORQUE  CONSTANTS 

TOX  =0.0 
TOY  =0.0 
TOZ  =  0.0 
T1X  =  0.0 
T1Y  =  0.0 
T1Z  =  0.0 
T2Y  =  0.0 
T2Z  =  0.0 

*INPUT  DISTANCE  FROM  CENTER  OF  LINK  TO  CENTER  OF  MASS  FOR  EACH  LINK  ENDS 
L ( 1 , 1 )  =  0.50 
L(l,2  =  0.50 

L(2,l  =  0.50 

L( 2 , 2 )  =  0.50 
L(3,l)  =  0.50 
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INPUT  HASS  AT  LINK  ENDS  IN  KILOGRAMS 


MASS! 

11, 1 

=  2.5 

MASSl 

1,2 

=  2.5 

MASS! 

2 , 1 

=  2.5 

MASSl 

2,2 

=  2.5 

MASSl 

3,1 

=  2.5 

MASSl 

3.2 

=  2.5 

INPUT  OMEGA  AND  OMEGA  DOT 
DO  30  I  =  1,3 


wxm 

=  0.0 

wy(i) 

=  0.0 

WZ(I) 

=  0.0 

WDX(  I 

=  0.0 

WDY  ( I 

=  0.0 

WDZ(  I 

=  0.0 

CONTINUE 


INPUT  INITIAL  VALUES  OF  EULER  ANGLE  THETA  AND  CONVERT  TO  RADIANS 
ETHETX(l)  =90.0 
TX1  =  ETHETX(l)  *  DEGRA 
ETHETY ( 1 )  =0.0 
TY1  =  ETHETY ( 1 )  *  DEGRA 
ETHETZ(l)  =  90.0 
TZ1  =  ETHETZ(l)  *  DEGRA 
ETHETX(2)  =90.0 
TX2  =  ETHETX(2 )  *  DEGRA 
ETHETY(2)  =0.0 
TY2=  ETHETY(2)  *  DEGRA 
ETHETZ(2 )  =  90.0 
TZ2  =  ETHETZ(2)  *  DEGRA 
ETHETX(3)  =90.0 
TX3  =  ETHETX(3)  *  DEGRA 
ETHETY ( 3 )  =45.0 
TY3  =  ETHETY(3)  *  DEGRA 
ETHETZ(3)  =  45.0 
TZ3  =  ETHETZ(3)  *  DEGRA 

INPUT  LOCATION  OF  LINK  CENTERS  OF  GRAVITY 
LCOGX(l)  =  0.0 
XI  =  LCOGX(l) 

LCOGY(l)  =0.5 
Y1  =  LCOGY(l) 

LCOGZ(l)  =0.0 
Z1  =  LCOGZ(l) 

LCOGX( 2 )  =0.0 
X2  =  LCOGX( 2 ) 

LCOGY ( 2 )  =1.5 
Y2  =  LCOGY ( 2 ) 

LCOGZ(2)  =  0.0 
Z2  =  LCOGZ (2 ) 

LCOGX(3)  =  0.0 
X3  =  LCOGX( 3 ) 

THERAD  =  ETHETY(3)  *  DEGRA 

LCOGY ( 3 )  =  2.0  +  COS (THERAD)  *  L ( 3 , 1 ) 

Y3  =  LCOGY ( 3 ) 

LCOGZ ( 3 )  =  L(3 , 1 )  *  SIN (THERAD) 

Z3  =  LCOGZ ( 3 ) 

INPUT  MASS  OF  EACH  LINK  IN  KG  AND  COMPUTE  WEIGHTS  IN  NEWTONS 
MAS SI  =  5.0 
MASS2  =  5.0 
MASS3  =5.0 
W1  =  MASS1*G 
W2  =  MASS2*G 
W3  =  MASS3*G 


*  INPUT  ACCELERATION  OF  JOINT  ZERO 

AOX  =  0.0 
AOY  =0.0 
AOZ  =  0.0 

DERIVATIVE 

NOSORT 

*  INPUT  JOINT  EQUATIONS 

*  INITIALIZE  MATRIX  B  TO  ZERO 

DO  10  I  =  1,27 
MATB(I)  =0.0 
10  CONTINUE 

*  INPUT  TORQUE  AT  JOINTS 

T2X  =  A*SIN  (W*TIME  +P) 

*  JOINT  ZERO  AB  =  AG1  +  (WD1  X  RB/G1 )  +  W1  X  (W1  X  RB/G1 ) 

VECTAO(l)  =  WDX(l) 

VECTAO (2 )  =  WDY(l) 

VECTAO ( 3 )  =  WDZ(l) 

RBG1(1)  =  JXO  -  LCOGX(l) 

RBG1 ( 2 )  =  JYO  -  LCOGY ( 1 ) 

RBG1 (3 )  =  JZO  -  LCOGZ(l) 

CALL  CPROD (VECTAO , RBG1 , MI AO , MJAO , MKAO ) 

VECTAO ( 1 )  =  WX(1) 

VECTAO ( 2 )  =  WY(1 
VECTAO ( 3 )  =  WZ(1) 

CALL  CPROD ( VECTAO , RBG1 , MIBO , M JBO , MKBO ) 

VECTBO ( 1 )  =  MIBO 
VECTBO  2  =  MJBO 

VECTBO (3)  =  MKBO 

CALL  CPROD ( VECTAO , VECTBO , MICO , MJCO , MKCO ) 

*  JOINT  ONE  EQUATIONS---  AA  =  AG1  +  (WD1  X  RA/G1 )  +  W1  X  (W1  X  RA/G1 ) 

VECTA1 (1 )  =  WDX(l) 

VECTA1 (2)  =  WDY ( 1 ) 

VECTA1 ( 3 )  =  WDZ(l) 

RAG1(1)  =  JX1  -  LCOGX(l) 

RAG1 (2 )  =  JY1  -  LCOGY ( 1 ) 

RAG1 ( 3 )  =  JZ1  -  LCOGZ(l) 

CALL  CPROD (VECTA1 , RAG1 , MIA1 , MJA1 , MKA1 ) 

VECTA1 ( 1 )  =  WX(1) 

VECTA1  ( 2 )  =  mil) 

VECTA1 ( 3 )  =  WZ(1) 

CALL  CPROD  (VECTA1 ,RAG1 ,MIB1 ,MJB1 ,MKB1) 

VECTB1 (1 )  =  MIB1 
VECTB1 ( 2 )  =  MJB1 
VECTB1 ( 3 )  =  MKB1 

CALL  CPROD  (VECTA1 , VECTB1 ,MIC1 ,MJC1 ,MKC1 ) 

*  AB  =  AG2  +  (WD2  X  RB/G2 )  +  W2  X  (W2  X  RB/G2) 

VECTA1 (1 )  =  WDX{2) 

VECTA1 ( 2 )  =  WDY (2) 

VECTA1 ( 3 )  =  WDZ(2) 

RBG2(1)  =  JX1  -  LCOGX( 2 ) 

RBG2(2)  =  JY1  -  LCOGY  2) 

RBG2 ( 3 )  =  JZ1  -  LCOGZ ( 2 ) 

CALL  CPROD  (VECTA1 , RBG2 ,MIA2 ,MJA2 ,MKA2 ) 

VECTA1 ( 1 )  =  WX( 2 ) 

VECTA1 ( 2 )  =  WY(2 
VECTA1 (3)  =  WZ(2) 

CALL  CPROD  (VECTA1 , RBG2 ,MIB2 ,MJB2 ,MKB2) 

VECTBl(l)  =  MIB2 
VECTE 1(2)  =  MJB2 
VECTB1 ( 3 )  =  MKB2 

CALL  CPROD  (VECTA1 , VECTB1 , MIC2 ,MJC2 ,MKC2 ) 


*  JOINT  TWO  EQUATIONS 

*  AA  =  AG2  +  (WD2  X  RA/G2 )  +  W2  X  (W2  X  RA/G2) 

VECTA2 ( 1 )  =  WDX(2) 

VECTA2(2)  =  WDY  2 
VECTA2(3)  =  WDZ(2) 

RAG2 ( 1 )  =  JX2  -  LCOGX( 2) 

RAG2 ( 2 )  =  JY2  -  LCOGY  2 
RAG2 ( 3 )  =  JZ2  -  LCOGZ(2) 

CALL  CPROD  (VECTA2 , RAG2 ,HIA3 , MJA3 , MKA3 ) 

VECTA2 ( 1 )  =  WX(2) 

VECTA2(2  =  WY( 2 ) 

VECTA2 ( 3 )  =  WZ ( 2 ) 

CALL  CPROD  (VECTA2 ,RAG2 ,MIB3 ,MJB3 ,MKB3) 

VECTB2 ( 1 )  =  MIB3 
VECTB2 ( 2 )  =  MJB3 
VECTB2 (3)  =  MKB3 

CALL  CPROD (VECTA2 , VECTB2 , MIC3 , MJC3 , MKC3 ) 

*  AB  =  AG3  +  (WD3  X  RB/G3)  +  W3  X  (W3  X  RB/G3 ) 

VECTA2 ( 1 )  =  WDX( 3) 

VECTA2 ( 2 )  =  WDY ( 3 ) 

VECTA2 ( 3 )  =  WDZ ( 3 ) 

RBG3 ( 1 )  =  JX2  -  LCOGX(3) 

RBG3 ( 2 )  =  JY2  -  LC0GY(3) 

RBG3 ( 3 )  =  JZ2  -  LCOGZ(3) 

CALL  CPROD  (VECTA2 , RBG3 ,MIA4 ,MKA4 ,MKA4 ) 

"ECTA2 ( 1 )  =  WX(3 ) 

VECTA2 ( 2 )  =  WY  3 
VECTA2 ( 3 )  =  WZ(3) 

CALL  CPROD  (VECTA2 , RBG3 ,MIB4 ,MJB4 ,MKB4) 

VECTB2 ( 1 )  =  MIB4 
VECTB2(2  =  MJB4 
VECTB2(3)  =  MKB4 

CALL  CPROD  (VECTA2 , VECTB2 ,MIC4 ,MJC4 ,MKC4) 

*  SUM  OF  MOMENTS  EQUATIONS 

*  CONVERT  EULER  ANGLES  FROM  DEGREES  TO  RADIANS 

DO  40  I  =  1,3 

THETXR(I)  =  ETHETX(I)  *  DEGRA 

THETYR(I)  =  ETHETY(I)  *  DEGRA 

THETZR(I)  =  ETHETZ(I)  *  DEGRA 

*  COMPUTE  HX  DOT ,HY  DOT, HZ  DOT 

RX( 1 , 1 )  =  -L(1 , 1 )  *  COS(THETXR(I) ) 

RX( I , 2 )  =  L(I ,2)  *  COS (THETXR( I ) ) 

RY ( I , 1 )  =  -L(I , 1)  *  COS (THETYR(I ) ) 

RY ( I , 2 )  =  L(I , 2 )  *  COS(THETYR( I ) ) 

RZ ( I , 1 )  =  -L ( I , 1 )  *  COS (THETZR( I ) ) 

RZ ( I , 2 )  =  L(I , 2)  *  COS (THETZR( I ' 

IXX( I , 1 )=MASS (I,1)*((RY(I, 1 ) *RY( I , 1 ) ) 

IXX(I , 2 )=MASS {I,2)*<(RY(I,2) *RY (1,2)) 

IXZ ( I , 1 )=MASS (1,1)  *  RZ ( I , 1 )  *  RX( I , 1 / 

IXZ ( I , 2 ) =MASS (1,2)  *  RZ  1,2)  *  RX(I,2) 

IXY( I , 1 ) =MASS (1,1)  *  RX  1,1)  *  RY ( I , 1 ) 

IXY (1,2) =MASS (1,2)  *  RX( I , 2 )  *  RY(I,2) 

HDX(l)  =  WDX(1 )*IXX(I,1) -WDZ (I )*IXZ(I,1) -WDY ( I 
HDX( 2 )  =  WDX( 2 ) *IXX (1,2) -WDZ (I)*IXZ(I ,2) -WDY ( I 
IYY (1,1)  =  MASS (I, l)  *  ( (RX( I , 1 ) *RX( I , 1 ) )  +  (RZ(I,1 

IYY (1,2)  =  MASS  1,2)  *  (RX(I,2)*RX(I ,2) )  +  (RZ(I,2 

I YZ (1,1/  =  MASS ( 1 , 1 )  *  RY ( 1 , 1 )  *  RZ ( 1 , 1 ) 

IYZ( I , 2 )  =  MASS (1,2)  *  RY ( I , 2 )  *  RZ(I,2) 

HDY(l)  =  WDY (l)*IYY(I,l) -WDX( I )*IXY(I,1) -WDZ (I)*IYZ(I,1 
HDY ( 2 )  =  WDY (I)*IYY(I ,2) -WDX ( I ) *IXY( I ,2) -WDZ( I )*IYZ(I ,2 
IZZ(I,1)  =  MASS (1,1)  *  ((RX(I,1)*RX(I,1))  +  ( RY ( I , 1) *RY ( I , 1 ) 

IZZ (1,2)  =  MASS (1,2)  *  ( (RX( I , 2 )*RX( I , 2 ) )  +  (RY( I , 2 )*RY (1,2) 

hdz(i)  =  wdz(i)*izz(i,i)-wdx(i)*ixz(i,i)-wdy(i)*iyz(i,i 

HDZ ( 2 )  =  WDZ (l)*IZZ(I ,2) -WDX( I )*IXZ(l,2) -WDY (l)*IYZ(I ,2 
IXXA(I)=MASS(I,2)*((L(I,2)+L(I,1))**2) 

SUMHDX(I)  =  HDX(l)  +  HD X(2) 


)  *IXY (1,1 
)*IXY(I ,2 

( x^Z ( 1 > 1 ) 

)*RZ(I ,2) 


SUMHDY 

I 

=  HDY(1 

)  +  HDY(2 ) 

SUMHDZ 

I) 

=  HDZ  ( 1 

)  +  HDZ( 2 ) 

CONTINUE 


TEST  TO  SEE  WHICH  CONSTRAINT  IS  IN  EFFECT  1,2  OR  3 


IF 

(RUN 

.EQ. 

1) 

GO 

TO 

1 

IF 

(RUN 

.EQ. 

2 

GO 

TO 

2 

IF 

(RUN 

.EQ. 

3) 

GO 

TO 

3 

INITIAI IZE  MATRIX  ACCORDING  TO  CONSTRAINT 
DO  60  I  =  1,18 

MATA( 1,1)  =1.0 
CONTINUE 
GO  TO  4 
DO  70  I  =  1,9 

MATA( 1,1)  =1.0 
CONTINUE 
GO  TO  7 

ENTER  CONSTANTS  INTO  MATRIX  A 
LINK  ONE 

SUM  OF  FORCES  IN  THE  X  DIRECTION 
MATA( 1,1)  =1.0 

MATA( 1,4)  =  MAS SI 
MATA(1 , 10)  =  -1.0 

SUM  OF  FORCES  IN  Y  DIRECTION 
MATA (2 , 2 )  =  1.0 
MATA (2 , 5 )  =  MAS SI 
MATA ( 2 ,11)  =  -1.0 

SUM  OF  FORCES  IN  Z  DIRECTION 
MATA( 3,3)  =1.0 
MATA (3,6)  =  MASS1 
MATA ( 3 , 12 )  =  -1.0 

SUM  OF  FORCES  LINK  ONE  EQUAL 
MATB ( 3 )  =  -W1 

EQUATIONS  AT  JOINT  ZERO 
IN  THE  X  DIRECTION 
MATA(4 , 4  )  =  1.0 
MATA (4 , 8 )  =  RBG1 ( 3 ) 

MATA(4 , 9 )  =  -RBG1 ( 2 ) 

MATB ( 4 ) =  AOX  -  MICO 

IN  THE  Y  DIRECTION 
MATA (5, 5)  =1.0 
MATA  ( 5 , 7 )  =  -RBG1(3) 

MATA ( 5 , 9 )  =  RBG1 { 1 ) 

MATB ( 5 )  =  AOY  -  MJCO 

IN  THE  Z  DIRECTION 
MATA (6,6)  =1.0 
MATA (6 , 7 )  =  RBG1 ( 2 ) 

MATA (6,8)  =  -RBGl(l) 

MATB ( 6 )  =  AOZ  -  MKCO 

SUM  OF  MOMENTS  EQUATIONS  FOR  LINK  ONE  IN  THE  X,Y,Z  DIRECTIONS 

MATA (7, 2)  =  RBG1 ( 3 ) 

MATA (7 , 3 )  =  -RBG1 (2) 

MATA  7,7  =  -(IXX(l.l)  +  IXX(1,2)) 

MATA  7,8)  =  I XY ( 1 , 1 )  +  IXY(1,2) 

MATA (7 ,9)  =  IXZ(1,1)  +  IXZ(1,2) 

MAT  A  ( 7 , 1 1 )  =  -RAG1 ( 3 ) 


MATA(7 , 12)  =  RAG1(2) 

MATB ( 7 )  =  T1X  -  TOX 

MATA (8,1)  =  -RBG1(3) 

MATA (8,3)=  RBG1(1) 

MATA(8 , 7 )  =  IXY (1,1)  +  IXY(1,2) 
MATA (8, 8)  =  - ( IYY (1,1)  +  IYY(1,2)) 
MATA  8,9)  =  IYZ (1,1)  +  IYZ(1,2) 
MATA (8 , 10)  =  RAG1 ( 3 ) 

MATA(8, 12)  =  -RAG1(1) 

MATB ( 8 )  =  T1Y  -  TOY 

MATA (9 , 1 )  =  RBG1 (2) 

MATA  9,2)  =  -RBGl(l) 

MATA  9,7)  =  IXZ(1,1  +  IXZ(1,2) 
MATA (9 , 8)  =  IYZ ( 1 , 1 )  +  IYZ(1,2) 
MATA (9 , 9 )  =  -<IZZ(1,1)  +  IZZ(1,2)) 
MATA  9,10)  =  -RAG1 ( 2 ) 

MATA ( 9 ,11)  =  RAG1(1) 

MATB (9)  =  T1Z  -  TOZ 

LINK  TWO 

SUM  OF  FORCES  IN  X  DIRECTION 
MATA(IO.IO)  =  1.0 

MATA ( 10 , 13 )  =  MASS2 
MATA ( 10 , 19 )  =  -1.0 

SUM  OF  FORCES  IN  THE  Y  DIRECTION 
MATA( 11 , 11 )  =  1.0 
MATA{11,14)  =  MASS2 
MATA(11,20)  =  -1.0 

SUM  OF  FORCES  IN  THE  Z  DIRECTION 
MATA( 12,12)  =  1.0 
MATA( 12 ,  15  j  =  MASS2 
MATA( 12 , 21 )  =  -1.0 

SUM  OF  FORCES  LINK  TWO  EQUAL 
MATB (12)  =  -W2 

EQUATIONS  AT  JOINT  ONE 
IN  THE  X  DIRECTION 
MATA( 13 , 4 )  =  -1.0 
MATAi 13,8)  =  -RAG1 ( 3 ) 

MATA(13,9)  =  RAG1 ( 2 ) 

MATA( 13,13)  =  1.0 
MATA( 13 , 17 )  =  RBG2 ( 3 ) 

MATA( 13 ,18)  =  -RBG2 ( 2 ) 

MATB (13)  =  MIC1  -  MIC2 

IN  THE  Y  DIRECTION 
MATA(14, 5)  =  -1.0 
MATA (14,7)  =  RAG1 ( 3 ) 

MATA(14,9)  =  -RAGl(l) 

MATA( 14 , 14)  =  1.0 
MATA( 14 , 16 )  =  -RBG2 ( 3 ) 

MATA( 14,18)  =  RBG2 ( 1 ) 

MATB ( 14)  =  MJC1  -  MJC2 

IN  THE  Z  DIRECTION 
MATA ( 1 5,6)  =  -1.0 
MATA  15,7  =  -RAG1 ( 2 ) 

MATA (15,8)  =  RAGl(l) 

MATA( 15 , 15)  =  1.0 


MATA(15 , 16)  =  RBG2(2) 
MATA( 15,17)  =  -RBG2 ( 1 ) 

MATB(15)  =  MKC1  -  MKC2 


SUM  OF  MOMENTS  EQUATIONS  FOR  LINK  TWO  IN  THE  X,Y,Z  DIRECTIONS 
MATA( 16 ,11)  =  RBG2(3) 

MATA  16,12)  =  -RBG2(2) 

MATA  16,16  =  -(IXX(2.1)  +  IXX(2  2)) 

MATA( 16 , 17  j  =  I XY ( 2 , 1 )  +  IXY  2,2 
MATA (16,18)  =  IXZ(2, 1)  +  IXZ(2,2) 

MATA( 16 , 20 
MATA (16 ,21 


I XY ( 2 , 1 
IXZ (2,1 
-RAG2 ( 3 
RAG2 ( 2 ) 


+  IXZ< 


nn  \  C.  / 

MATB(16)  =  -T1X  +  T2X 
I F ( RUN  .EQ.  2)  GO  TO  11 

MATA(17 ,10)  =  -RBG2(3) 

MATAi 17 , 12)  =  RBG2 ( 1 ) 

MATA  17,16  =  IXY  21)  +  IXY(2  2) 

MATA (17 , 17 j  =  - ( IYY(2 , 1 )  +  IYY(2  2)) 
MATA( 17 , 18 )  =  IYZ( 2 , 1 )  +  IYZ(2,2) 
MATA ( 17 ,19)  =  RAG2(3) 

MATA( 17 , 21 )  =  -RAG2 ( 1 ) 


MATB( 17 )  =  - 


MATA1 

MATA 

MATA 

MATA 

MATA 

MATA 

MATA 


18,10) 

18,11) 

18,16 

18.17 

18.18 
18,i9 
18, 20] 


RBG2(2) 
-RBG2 ( 1 ) 
IXZ(2,1 
IYZ(2  1) 

:m 

RAG2 ( 1 ) 


il;i! 


1)  +  IZZ(2 , 2) ) 


MATB( 18)  =  -  T1Z  +  T2Z 

IF  (RUN  .EQ.  3)  GO  TO  4 
MATA( 17 , 17 )  =1.0 
MATA( 18 , 18 )  =  1.0 

LINK  THREE 

SUM  OF  FORCES  IN  THE  X  DIRECTION 
MATA (19,19)  =  1.0 

MATA ( 19 , 22 )  =  MASS3 

SUM  OF  FORCES  IN  THE  Y  DIRECTION 
MATA(20 , 20 )  =1.0 
MATA(20 , 23 )  =  MASS3 

SUM  OF  FORCES  IN  THE  Z  DIRECTION 
MATA( 21 , 21 )  =  1.0 
MATA(21 , 24 )  =  MASS3 

MATB (21)  =  -W3 

EQUATIONS  AT  JOINT  TWO 
IN  THE  X  DIRECTION 


MATA ( 22 , 13 
MATA(22 , 17 
MATA (22 , 18 
MATA (22, 22 


=  -1.0 
=  -RAG2 (3 ) 
=  RAG2 ( 2 ) 

=  1.0 


MATA (22, 26  =  RBG3(3) 

MATA(22 , 27 )  =  -RBG3(2) 

MATB(22)  =  MIC3  -  MIC4 

IN  THE  Y  DIRECTION 

MATA (23 ,14)  =  -1.0 
MATA( 23 , 16 )  =  RAG2(3) 


MATA (23 ,18)  =  -RAG2(1) 

MATA  23,23  =1.0 

MATA  23,25)  =  -RBG3(3) 

MATA(23 , 27 )  =  RBG3(1) 

MATB(23)  =  MJC3  -  MJC4 

*  IN  THE  Z  DIRECTION 

MATA( 24 ,15)  =  -1.0 
MATA ( 24 , 16 )  =  -RAG2(2) 

MATA(24,17  =  RAG2(1) 

MATA( 24 , 24 )  =1.0 
MATA ( 24 , 25 )  =  RBG3(2) 

MATA( 24 , 26 )  =  -RBG3(1) 

MATB(24)  =  MKC3  -  MKC4 

*  SUM  OF  MOMENTS  EQUATIONS  FOR  LINK  THREE  IN  THE  X, Y,Z  DIRECTIONS 

MATA{ 25 , 20 )  =  RBG3(3) 

MATA  25,21  =  -RBG3(2) 

MATA ( 25 , 25 )  =  -(IXX(3,1)  +  IXX(3.2)) 

MATA( 25,26)  =  IXY(3,l)  +  IXY(3,2) 

MATA(25 . 27 )  =  1X2(3, 1)  +  IXZ(3,2) 

MATB (25)  =  -  T2X 

IF (RUN  .EQ.  1  .OR.  RUN  .EQ.  2)  GO  TO  12 

MATA ( 26 , 19 )  =  -RBG3(3) 

MATA (26 , 21 )  =  RBG3(1) 

MATA ( 26 , 25 )  =  IXY(3,1)  +  IXY(3,2) 

MATA( 26 , 26 )  =  -(IYY(3,1)  +  IYY(3.2)) 

MATA(26 , 27 )  =  IYZ(3,1)  +  IYZ(3,2) 

MATB(26)  =  -  T2Y 

MATA ( 27 , 19 )  =  RBG3(2) 

MATA( 27,20)  =  -RBG3(1) 

MATA  27,25  =  IXZ(3,1  +  IXZ 

MATA(27 , 26 )  =  IYZ(3,1)  +  IYZ 
MATA (27 , 27 )  =  -(IZZ(3,1)  +  I 

MATB(27 )  =  -  T2Z 

IF  (RUN  .EQ.  3)  GO  TO  13 
12  MATA(26 , 26)  =  1.0 

MATA(27,27)  =  1.0 


*  CALL  EQUATION  SOLVER  PROGRAM  FROM  IMSL 

13  CALL  LEQT2F (MATA,M ,N , IA ,MATB , IDGT , WKAREA, IER) 

IF  (IER  .NE.  0)  CALL  ENDJOB 

*  FIND  LCOGX , LCOGY , LCOGZ , THETA  VALUES ,WX,WY,WZ 

IF (RUN  .EQ.  1)  GO  TO  6 
IF  (RUN  .EQ.  2)  GO  TO  9 

*  LINK  ONE 

AX1  =  MATB(4) 

VELX1  =  INTGRL ( 0 , AX1 ) 

LCOGX1  =  INTGRL ( XI, VELX1) 

LCOGX ( 1 )  =  LCOGX1 
AY1  =  MATB(5) 

VELY1  =  INTGRL (0,AY1) 

LCOGY1  =  INTGRL(Y1 ,VELY1 ) 

LCOGY ( 1 )  =  LCOGY1 
AZ1  =  MATB ( 6 ) 

VELZ1  =  INTGRL (0 , AZ1 ) 

LCOGZ1  =  INTGRL (Z1 , VELZ1 ) 

LCOGZ ( 1 )  =  LCOGZ1 
WD1X  =  MATB (7 ) 


ZZ ( 3 , 2 ) ) 


W1X  =  INTGRL(0,WD1X) 

THEXR1  =  INTGRL(TY1,W1X) 

JXO=  LCOGX(l)  -  L( 1 , 1 )  *  COS(TXl) 
WDX(l)  =  WD1X 
WX(1)  =  W1X 

CTHETX ( 1 )  =  THEXR1  *  RADEG 
ETHETY ( 1 )  =  CTHETX ( 1 ) 

WD1Y  =  MATB (8) 

W1Y  =  INTGRL(0 ,WD1Y) 

THEYR1  =  INTGRL(0 . ,W1Y) 

JYO  =  LCOGY(l)  -  L( 1 , 1 )  *  COS (THEXR1 ) 
WDY(l)  =  WD1Y 
WY ( 1 )  =  W1Y 

CTHETY ( 1 )  =  THEYR1  *  RADEG 
WD1Z  =  MATB(9) 

W1Z  =  INTGRL(0 , WD1Z) 

THEZR1  =  INTGRL(0 . ,W1Z) 

WDZ(l)  =  WD1Z 
WZ ( 1 )  =  W1Z 

CTHETZ(l)  =  THEZR1  *  RADEG 
ETHETZ(l)  =  90.0  -  CTHETX(l) 

ETHEZ1  =  ETHETZ(l)  *  DEGRA 

JZO  =  LCOGZ(l)  -  L(  1 , 1 )  *  COS (ETHEZ1 ) 

LINK  TWO 

AX2  =  MATB(13 ) 

VELX2  =  INTGRL ( 0 , AX2 ) 

LCOGX2  =  INTGRL (X2,VELX2) 

LCOGX(2)  =  LCOGX2 
AY2  =  MATB ( 14 ) 

VELY2  =  INTGRL (0,AY2) 

LCOGY2  =  INTGRL (Y2.VELY2) 

LCOGY (2)  =  LCOGY2 
AZ2  =  MATB ( 15 ) 

VELZ2  =  INTGRL (0,AZ2) 

LCOGZ2  =  INTGRL (Z2.VELZ2) 

LCOGZ ( 2 )  =  LCOGZ2 
WD2X  =  MATB( 16 ) 

W2X  =  INTGRL (0,WD2X) 

THEXR2  =  INTGRL (TY2,W2X) 

JX1  =  LCOGX(2)  -  L(2 , 1 )  *  COS(TX2) 
WDX(2)  =  WD2X 
WX( 2 )  =  W2X 

CTHETX(2)  =  THEXR2  *  RADEG 
ETHETY(2)  =  CTHETX(2) 

WD2Y  =  MATB (17) 

W2Y  =  INTRGL(0 ,WD2Y) 

THEYR2  =  INTGRL ( 0. ,W2Y) 

JY1  =  LCOGY ( 2 )  -  L  ( 2 , 1 )  *  COS(THEXR2) 
WDY ( 2 )  =  WD2Y 
WY( 2 )  =  W2Y 

CTHETY ( 2 )  =THEYR2  *  RADEG 
WD2Z  =  MATB (18) 

W2Z  =  INTGRL(0 ,WD2Z) 

THEZR2  =  INTGRL ( 0. ,W2Z) 

WDZ(2 )  =  WD2Z 
WZ(2)  =  W2Z 

CTHETZ(2)  =  THEZR2  *  RADEG 
ETHETZ( 2 )  =  90.0  -  CTHETX(2) 

ETHEZ2  =  ETHETZ(2)  *  DEGRA 

JZ1  =  LCOGZ ( 2 )  -  L f 2 , 1 )  *  COS(ETHEZ2) 

LINK  THREE 

AX3  =  MATB (22) 

VELX3  =  INTGRL ( 0. ,AX3) 

LCOGX3  =  INTGRL (X3 , VELX3 ) 

LCOGX( 3 )  =  LCOGX3 
AY 3  =  MATB (23) 

VELY3  =  INTGRL ( 0. ,AY3) 


LC0GY3  =  INTGRL( Y3 , VELY3 ) 

LCOGY ( 3 )  =  LCOGY3 
AZ3  =  MATB(24) 

VELZ3  =  INTGRL (0 . , AZ3 ) 

LCOGZ3  =  INTGRL(Z3 , VELZ3) 

LCOGZ(3)  =  LCOGZ3 
WD3X  =  MATB ( 25 ) 

W3X  =  INTGRL ( 0 . , WD3X ) 

THEXR3  =  INTGRL(TY3 ,W3X) 

JX2  =  LCOGX( 3 )  -  L( 3 , 1 )  *  COS(TX3) 
WDX( 3 )  =  WD3X 
WX( 3 )  =  W3X 

CTHETX ( 3 )  =  THEXR3  *  RADEG 
ETHETY ( 3 )  =  CTHETX(3) 

WD3Y  =  MATB (26) 

W3Y  =  INTGRL(0 . ,WD3Y) 

THEYR3  =  INTGRL ( 0. ,W3Y) 

JY2  =  LCOGY ( 3 )  -  L(3,l)  *  COS(THEXR3) 
WDY ( 3 )  =  WD3Y 
WY ( 3 )  =  W3Y 

CTHETY(3)  =  THEYR3  *  RADEG 
WD3Z  =  MATB(27) 

W3Z  =  INTGRL (0 . ,WD3Z) 

THEZR3  =  INTGRL(0. ,W3Z) 

WDZ ( 3 )  =  WD3Z 
WZ ( 3 )  =  W3Z 

CTHETZ ( 3 )  =  THEZR3  *  RADEG 
ETHETZ(3)  =  90.0  -  CTHETX(3) 

ETHEZ3  =  ETHETZ(3 )  *  DEGRA 

JZ2  =  LCOGZ( 3 )  -  L  ( 3 , 1 )  *  COS(ETHEZ3) 

DYNAMIC 


THEORY (3)=( ( ( (—2-5/ (PI*PI))*SIN (W*TIME ) )+(5.*TIME)/PI)/ IXXA ( 3 ) ) . 
+  PI/4 . 

EULORY ( 3 )  =  THEORY ( 3 )  *  RADEG 
THDDOT ( 3 )  =  T2X/IXXA(3) 

ERROR(3)  =  ( (ABS(EULORY(3)-ETHETY(3) ) )/  81.4/6)*100. 


END 

STOP 

FORTRAN 

*  SUBROUTINE  TO  COMPUTE  THE  CROSS  PRODUCT  OF  TWO  VECTOR 

SUBROUTINE  CPROD( VECTA , VECTB ,MI , MJ ,MK) 

IMPLICIT  REAL*8  (A-Z) 

DIMENSION  VECTA(3) ,VECTB(3) 

MI  =  VECTA(2)  *  VECTB ( 3 )  -  VECTA(3)  *  VECTB(2) 

MJ  =  VECTA  3  *  VECTB ( 1 )  -  VECTA  1  *  VECTB(3) 

MK  =  VECTA(l)  *  VECTB ( 2 )  -  VECTA(2)  *  VECTB(l) 

RETURN 
END 


I 


APPENDIX  B 


SIMULATION  PROGRAM  FOR  MOVEMENT  OF  LINK  TWO  AND 

THREE 


TERMINAL 
METHOD  ADAMS 

PRINT  .03 ,  T1X , T0RY1X , T2X , TORY2X ,ERRT2X,ERRT1X, ETHETY (2-3) 

CONTROL  FINTIM  =3.0,  DELMAX  =.01,  DELPRT  =  .03 

SAVE  . 01 , ERRT2X, ERRT1X , TORY1X , T0RY2X , T1X , T2X , ETHETY ( 2 ) , ETHETY ( 3 ) 

GRAPH ( DE=TEK6 1 8 )  TIME , T1X , T0RY1X 

GRAPH  DE=TEK618)  TIME , T2X , T0RY2X 

GRAPH  DE=TEK618  TIME , ERRT2X 

GRAPH  DE=TEK618  TIME , ERRT1X 

GRAPH ( DE=TEK6 1 3 )  TIME , ETHETY ( 3 ) , ETHETY ( 2 ) 

D  DIMENSION  MATA (27 .27) , MASS (3, 2) ,L(3/2),RX(3,2),RY(3,2),RZ(3,2) 

D  DIMENSION  I XX (3, 2) , IXZ ( 3 , 2 ) , IXY( 3 , 2 ) , I YY ( 3 , 2 ) , IYZ ( 3 , 2 ) ,IZZ(3,2) 

D  INTEGER  IER , I , RUN , M , N , IA , IDGT 
EXCLUDE  I A , IDGT , IER , I , RUN , M ,N 

ARRAY  MATB ( 27 ) . LCOGX ( 3 ) , LCOGY ( 3 ) , LCOGZ ( 3 ) , ETHETX ( 3 ) , ETHETY ( 3 ) , ETHETZ ( 3 ) 
ARRAY  CTHETX( 3 ) , CTHETY ( 3 ) , CTHETZ (3 ) 

ARRAY  VECTAO ( 3 ) , VECTBO ( 3 ) . VECTA1 ( 3 ) ,VECTB1(3) ,VECTA2(3) ,VECTB2(3) 

ARRAY  WDX ( 3 ) . WDY ( 3 ) , WDZ ( 3 ) , WX ( 3 ) , WY ( 3 ) , WZ ( 3 ) ,RBG1(3) ,RAG1(3) 

ARRAY  RBG2 ( 3 ) ,RAG2(3) , RBG3 ( 3 ) , THETXR ( 3 ) , THETYR ( 3 ) , THETZR( 3 ) 

ARRAY  SUMHDX ( 3 ) ,SUMHDY(3) ,SUMHDZ(3) , HDX( 2 ) , HDY( 2 ) , HDZ ( 2 ) , WKAREA( 850 ) 

D  DATA  MATA/729  *  0./ 

INITIAL 

*  INPUT  PARAMETER  CONSTANTS 

A  =  2.0 
P  =  0.0 
W  =  2‘PI 
IDGT  =  4 
G=Q .  0 
N=27 
M=1 

IA  =27 
RUN  =  2 

*  INPUT  JOINT  LOCATIONS  IN  METERS 

JXO  =0.0 
JYO  =0.0 
JZO  =  0.0 
JX1  =  0.0 
JY1  =1.0 
JZ1  =  0.0 
JX2  =  0.0 
JY2  =  2.0 
JZ2  =  0.0 

*  INPUT  TORQUE  CONSTANTS 

TOX  =0.0 
TOY  =0.0 
TCZ  =  0.0 
T1Y  =  0.0 
TiZ  =  0.0 
TZY  =0.0 
T2Z  =  0.0 

‘INPUT  DISTANCE  FROM  CENTER  OF  LINK  TO  CENTER  OF  MASS  FOR  EACH  LINK  ENDS 


%  N 


* 


k 


S, 


i  + 


30 


L( 3 , 1 )  =  0.50 
L(3 , 2 )  =  0.50 

INPUT  HASS  AT  LINK  ENDS  IN  KILOGRAMS 


MASS (1,1)  = 

2.5 

MASS (1,2)  = 

2.5 

MASS (2,1)  = 

2.5 

MASS (2,2)  = 

2.5 

MASS (3,1)  = 

2.5 

MASS (3,2)  = 

2.5 

INPUT  OMEGA  AND  OMEGA  DOT 
DO  30  I  =  1,3 


WX(I) 

=  0.0 

WY  I 

=  0.0 

WZ(I) 

=  0.0 

WDX(  I 

=  0.0 

WDY  I 

=  0.0 

WDZ(  I 

=  0.0 

CONTINUE 


INPUT  INITIAL  VALUES  OF  EULER  ANGLE  THETA  AND  CONVERT  TO  RADIANS 
ETHETX(l)  =90.0 
TX1  =  ETHETX(l)  *  DEGRA 
ETHETY ( 1 )  =0.0 
TY1  =  ETHETY ( 1 )  *  DEGRA 
ETHETZ(l)  =90.0 
TZ1  =  ETHETZ(l)  *  DEGRA 
ETHETX( 2 )  =90.0 
TX2  =  ETHETX(2)  *  DEGRA 
ETHETY ( 2 )  =0.6 
TY2=  ETHETY ( 2 )  *  DEGRA 
ETHETZ(2)  =90.0 
TZ2  =  ETHETZ ( 2 )  *  DEGRA 
ETHETX(3 )  =90.0 
TX3  =  ETHETX  3)  *  DEGRA 
ETHETY ( 3 )  =  45.0 
TY3  =  ETHETY ( 3 )  *  DEGRA 
ETHETZ ( 3 )  =45.0 
TZ3  =  ETHETZ( 3 )  *  DEGRA 

INPUT  LOCATION  OF  LINK  CENTERS  OF  GRAVITY 
LCOGX(l)  =0.0 
XI  =  LCOGX(l) 

LCOGY(l)  =0.5 
Y1  =  LCOGY(l) 

LCOGZ(l)  =0.0 
Z1  =  LCOGZ(l) 

LCOGX( 2 )  =0.0 
X2  =  LCOGX( 2 ) 

LCOGY ( 2  )  =  1.5 
Y2  =  LCOGY ( 2 ) 

LCOGZ ( 2  )  =0.0 
Z2  =  LCOGZ ( 2 ) 

LCOGX( 3 )  =0.0 
X3  =  LCOGX ( 3 ) 

THERAD  =  ETHETY ( 3 )  *  DEGRA 

LCOGY ( 3 )  =  2.0  +  COS (THERAD)  *  L(3,l) 

Y3  =  LCOGY ( 3 ) 

LCOGZ ( 3 )  =  L ( 3 , 1 )  *  SIN( THERAD ) 

Z3  =  LCOGZ ( 3 ) 

INPUT  MASS  OF  EACH  LINK  IN  KG  AND  COMPUTE  WEIGHTS  IN  NEWTONS 
MASS  1  =  5.0 
HASS  2  =  5.0 
MAS  S3  =  5.0 
W1  =  MASS  1 *G 
W2  =  MASS2*G 
W3  =  MASS  3*0 


t's-s  -■ 


FVS\  «r„ 


V  V  «"  V*  Ji  1 


■  '  i  *  . 


.V.V.V.  V  V 


-  r  v 


INPUT  ACCELERATION  OF  JOINT  ZERO 
AOX  =  0.0 
AOY  =  0.0 
AOZ  =  0.0 


DERIVATIVE 


*  INPUT  JOINT  EQUATIONS 

NOSORT 


10 

■k 


INITIALIZE  MATRIX  B  TO  ZERO 
DO  10  I  =  1,27 
MATB(I)  =0.0 
CONTINUE 


INPUT  TORQUE  AT  JOINTS 

T2X  =  - A*S IN  (W*TIME  +  P) 
T1X  =  A*S  IN  (W*TIME  +  P) 


JOINT  ZERO  AB  =  AG I  +  (WD1  X  RB/Glj  +  W1  X  (W1  X  RB / G I 
VECTAO ( 1 )  =  WDX(l) 

VECTAO ( 2 )  =  WDY(l) 

VECTAO ( 3  )  =  WDZU  ) 

RBG1 ( 1 j  =  JXO  -  LCOGXI I ) 

RBG1  2)  =  JYO  -  LCOGY(l) 

RBG1  (  3  )  =  JZO  -  LCOGZm 
CALL  CPROD ( VECTAO , RBG I , MI AO , MJAO , MKAQ ) 

VECTAO ( 1 )  =  WX( 1 ) 

VECTAQf’ 2 ;  =  WY (  I ) 

VECTAO ( 3  ;  =  WZi 1) 

CALL  CPROD (VECTAO  PBG1 , MIBO . MJBO  MKBO / 

1 1  BO 
.1 JBO 


VECTBO ( 1  )  =  MI 
VECTBO  (  2  )  =  .!  J 


VECTBO ( 3  )  =  MKB: 

CALL  CPROD (VECTAO . VECTBC 


JOINT  ONE  EQUATIONS---  AA  =  AG; 

VECTA1 1 1 ;  =  WDX  ■  1 
VE CT A 1  (  2 )  =  WDY  ■  I 


,  MI  CO . M J CO  MX 

• WD ;  X  rA  -  ;  , 


w  1  X  w ;  x  ?  a 


'  t  . .  rt  i 


)  =  wcz- 


-  j  r.  i  -  i.  . 

=  ;v;  -  l: 


.  rt  i  r  n  j  , 


F  A  j  1  S  i 
F  A  j  i  '  L. 

rag: '3  = 

CALL  CrPOCiVE 

VE  CTAi  •'  i  .  =  «> 
VECTA1(2)  =  w*i 
VE  ITA 1  ■  3  -  =  w; 
CALL  “FF  I  VECTA 

vectb;  ■ ;  -  :*:  = ; 

-'E  “TB  ;  2  -  u  '  E  . 

VECTB1  ;  -  MX 5. 

CALL  CPROD  '  VE  'TA I  VE 


M I A ;  MCA 


MX  A 


FA 


MIS.  M 


MXB. 


AB  -  AG* 


:e 

VE 


.  t  .  i“. . 

FE  .r . 


CALL  CPROD  (VECTA1 , VECTB1 ,MIC2 ,MJC2 , MKC2) 


*  JOINT  TWO  EQUATIONS 

*  AA  =  AG2  +  (WD2 


(WD2  X  RA/G2 )  +  W2  X  (W2  X  RA/G2) 


VECTA2 ( 1 )  =  WDX(2) 

VECTA2 ( 2 )  =  WDY  2 
VECTA2(3)  =  WDZ ( 2 ) 

RAG2 ( 1 )  =  JX2  -  LCOGX(2] 
RAG2 ( 2 )  =  JY2  -  LCOGY  2 
RAG2 ( 3 )  =  JZ2  -  LCOGZ  2 


RAG2 ( 1 ] 
RAG2(2 
RAG2 ( 3 


CALL  CPROD  (VECTA2 , RAG2 ,MIA3 ,MJA3 ,MKA3 ) 
VECTA2 ( I )  =  WX(2) 

VECTA2 (2)  =  WY  2) 

VECTA2 ( 3 )  =  WZ  ( 2 ) 

CALL  CPROD  (VECTA2 , RAG2 ,MIB3 ,MJB3 ,HKB3 ) 
VECTB2 ( 1 )  =  MIB3 
VECTB2 ( 2 )  =  MJB3 
VECTB2 ( 3 )  =  MKB3 

CALL  CPROD (VECTA2 , VECTB2 , MIC3 , MJC3 , MKC3 ) 


AG3  +  ( WD3  X  RB/G3 )  +  W3  X  (W3  X  RB/G3) 
VECTA2 ( 1 )  =  WDX ( 3 ) 

VECTA2  2  =  WDY  3) 

VECTA2 ( 3 )  =  WDZ( 3 ) 

RBG3 ( 1 )  =  JX2  -  LCOGX(3) 

RBG3(2  =  JY2  -  LCOGY(3 
RBG3 ( 3 )  =  JZ2  -  LCOGZ  3 


CALL  CPROD  (VECTA2 , RBG3 ,MIA4 ,MKA4,MKA4) 
VECTA2 ( I )  =  WX(3) 

VECTA2  2  =  WY(3 

VECTA2  (  3 )  =  WZ  ( 3 ) 

CALL  CPROD  ( VECTA2 , RBG3 ,MIB4 , MJB4 , MKB4 ) 
VECTB2 ( 1 )  =  MIB4 
VECTB2  2  =  MJB4 

VECTB2 ( 3 )  =  MKB4 


CALL  CPROD  (VECTA2 , VECTB2 ,MIC4 ,MJC4 ,MKC4) 


SUM  OF  MOMENTS  EQUATIONS 

CONVERT  EULER  ANGLES  FROM  DEGREES  TO  RADIANS 

DC  40  I  =  1.3 

THETXR ( I )  =  ETHETX(I)  *  DEGRA 
THETYR  (’  I  )  =  ETHETY  I  *  DEGRA 
THETZR(I)  =  ETHETZ ( I )  *  DEGRA 


.MPUTE  HX  H  DOT 


:xx'  i 
I XZ '  I 
IXZ  I 
in  i  i 


mi: 


H  DOT  X.HY.H  DOT  Y, 
RX< I . 1 )  =  -L ( I  ,  I )  * 
PXi I .2  =  L<  I  . 2)  * 

RY< I . 1 )  =  - L ( I  ,  1  )  * 
RY  (1.2)  =  HI,  2)  * 
FZi I  I )  =  -L(  I  ,  1  )  * 
RZ i 1,2)  ^  1(1,2)  * 

1  =MASS( I . i )*( (RY(I 
2 ) =MA5S ( I . 2)*( ( RY( I 
1 ')  =MASS  (  I  .  1  )  *  RZ  f  I 
2 , =MASS  1.2)  *  RZ (  I 

1  =MASS  (  I . i )  *  R X !  I 

2  -MASS ( 1.2;  *  PX,  I 


=  WCXU)*IXX( 


HZ , H  DOT  Z 
COS ( THETXR( I ) ) 
COS ( THETXR ( I ) 

COS (THETYR (I  ) 
COS (THETYR (I ) 

COS ( THETZR ( I )  ) 
COS ( THETZR( I ) ) 

, 1)*RY(I , 1  j+  RZl 
, 2 )*RY( I , 2 ) )+(RZi 
. I )  *  RX ( I , 1  ) 

,2)  *  RX ( I , 2 ) 

.  1 }  *  RY ( I  ,  1 
2)  *  RY(I ,2) 


OT:Jlli 


i  =  WDX i 
MASS'  I 
MASS ! I 
MASS'  I 


,'*IXX( 

.  >  *  (  < 


1 . 1 )  -WDZ (I ) *IXZ ( 

1 . 2 )  -WDZ ( I ) *IXZ( 


PX'I  1 ) *RX (1,1)) 
PX ! I  2  *RX(I .2)  ) 
'I  Li  *  RZ < I . 1  ) 

I  2 .  *  R  Z i 1.2; 

I  i  -WDX ' I ; *IXY < I 
I  2 i -WDX'  I  1  *  I XY ' I 
px'i  i  !*?:<(  i .  i , ) 
PX' :  2  !  * P X ' i .  2  ' ) 


) -WDY ( 

:Wwdy( 

(RZl I , 
( RZ ( I  , 


I ) *  I XY ( 
I ) *  I XY ( 

1 )  *rz  ( : 

2) *rzi: 


) -WDZ; 
> -WDZ ( 
( RY ( I  . 
■  P.Y(  I  , 
! -WDY ( 
i  -  WDY I 


I )*IYZ( 


I  i  *  I YZ  1 

:  i  *ry i 

2 ' *FY< 

I  .  *IYZ 
I  i  *  I  YZ 


SUMHDX(I)  =  HDX(l)  +  HDX(2) 

SUMHDY(I)  =  HDY(l)  +  HDY  2 
SUMHDZ(I)  =  HDZ ( 1 )  +  HDZ(2) 

CONTINUE 

TEST  TO  SEE  WHICH  CONSTRAINT  IS  IN  EFFECT  1,2  OR  3 
IF  (RUN  .EQ.  1)  GO  TO  1 

IF  (RUN  .EQ.  2)  GO  TO  2 

IF  (RUN  .EQ.  3)  GO  TO  3 

INITIAIIZE  MATRIX  ACCORDING  TO  CONSTRAINT 
DO  60  I  =  1,18 

MATA (I , I)  =  1.0 
CONTINUE 
GO  TO  4 
DO  70  I  =  1,9 

MATA ( I , I )  =1.0 
CONTINUE 
GO  TO  7 

ENTER  CONSTANTS  INTO  MATRIX  A 
LINK  ONE 

SUM  OF  FORCES  IN  THE  X  DIRECTION 
MATA( 1,1)  =1.0 

MATA ( 1 ,4)  =  MASS1 
MATA(l,10)  =  -1.0 

SUM  OF  FORCES  IN  Y  DIRECTION 
MATA ( 2 , 2 )  =1.0 
MATA (2,5;  =  MASS1 
MATA ( 2 , 11 )  =  -1.0 

SUM  OF  FORCES  IN  Z  DIRECTION 
MATA ( 3 , 3 )  =  1.0 
MATA (3, 6)  =  MAS SI 
MATA (3 ,12)  =  -1.0 

SUM  OF  FORCES  LINK  ONE  EQUAL 
MATB ( 3 )  =  -W1 

EQUATIONS  AT  JOINT  ZERO 
IN  THE  X  DIRECTION 
MATA (4 ,4)  =  1.0 
MATA (4, 8;  =  RBG1(3) 

MATA(4,9)  =  -RBGl(2) 

MATB(4)=  AOX  -  MICO 

IN  THE  Y  DIRECTION 
MATA( S , 5 )  =1.0 
MATA ( 5 , 7 )  =  -RBG1 ( 3 ) 

MATA ( 5 , 9 ,  =  IBGl(l) 

MATB ( 5 )  =  AOY  -  MJCO 

IN  THE  Z  DIRECTION 
MAT  A  ( 6 , 6 )  =  1.0 
MATA (6 ,7 )  =  RBG1 ( 2 ) 

MATA (6, 8)  =  -RBGl(l) 

MATB ( 6 )  =  AOZ  -  MKCO 

SUM  OF  MOMENTS  EQUATIONS  FOR  LINK  ONE  IN  THE  X,Y,Z  DIRECTIONS 

MATA (7 , 2)  =  RBG1 ( 3 ) 

MATA  7,3  =  -RBG1 ( 2 ) 

MATA  7,7)  =  -(IXX(l.l)  +  IXX(1,2)) 

MATA  7,8)  =  IXY ( 1 , 1 )  +  IXY (1,2) 

MATA  7,9)  =  IXZ  1,1)  +  IXZ  1,2) 


MATA (7,11)  =  -RAG1(3) 

MATA (7 ,12)  =  RAG1(2) 

MATB ( 7 )  =  T1X  -  TOX 

MATA(8 , 1 )  =  -RBG1 (3) 

MATAi 8 , 3)=  RBG1(1) 

MATAi 8 , 7 )  =  IXY (1,1)  + 
MATA(8,8  =  - ( IYY (1,1) 

MATAi 8 , 9 )  =  IYZ(l,l)  + 

MATAi 8, 10)  =  RAG1 ( 3 ) 
MATA(8,12)  =  -RAG1(1) 

MATB ( 8 )  =  T1Y  -  TOY 

MATA (9,1)  =  RBG1 (2) 

MATA (9, 2)  =  -RBG1(1) 

MATA (9, 7)  =  IXZ(l,l)  + 
MATA(9 , 8 )  =  IYZ (1,1)  + 

MATA  9,9)  =  -(IZZ(1,1) 

MATA  9,10)  =  -RAG1 (2) 

MATA(9 , 11 )  =  RAG1(1) 

MATB ( 9 )  =  T1Z  -  TOZ 

LINK  TWO 

SUM  OF  FORCES  IN  X  DIRECTION 
MATA( 10,10)  =  1.0 

MATA (10,13)  =  MASS2 
MATA( 10,19)  =  -1.0 

SUM  OF  FORCES  IN  THE  Y  DIRECTION 
MATA (11,11)  =  1.0 
MATAi 11 , 14)  =  MASS2 
MATA( 11 , 20 )  =  -1.0 

SUM  OF  FORCES  IN  THE  Z  DIRECTION 
MATA( 12,12)  *  1.0 
MATA  12,15  =  MASS2 

MATA ( 12 , 21 )  =  -1.0 

SUM  OF  FORCES  LINK  TWO  EQUAL 
MATB (12)  =  -W2 

EQUATIONS  AT  JOINT  ONE 
IN  THE  X  DIRECTION 
MATA (13 ,4)  =  -1.0 
MATA( 13 , 8 )  =  -RAG1 ( 3 ) 

MATAi 13,9)  =  RAG1 ( 2 ) 

MATAi 13,13)  =  1.0 
MATA( 13,17)  =  RBG2 ( 3 ) 

MATA( 13,18)  =  -RBG2(2) 

MATB ( 13 )  =  MIC1  -  MIC2 

IN  THE  Y  DIRECTION 
HATA( 14 , 5 )  =  -1.0 
MATAi 14 , 7 )  =  RAG1( 3) 

MATAi 14,9)  =  -RAGl(l) 

MATAI 14 , 14)  =  1.0 
MATAi 14 , 16 )  =  -RBG2 ( 3 ) 

MATA( 14,18)  =  RBG2 ( 1 ) 

MATB( 14)  =  MJC1  -  MJC2 


IXY (1,2) 

+  IYY (1.2)) 
IYZ (1,2) 


IYZ  (  j 
+  IZZ(1 ,2) ) 


IN  THE  Z  DIRECTION 
MATA (15, 6)  =  -1.0 
MATA  15,7  =  -RAG1 ( 2 ) 

MATAi 15,8)  =  RAGl(l) 


*  * 


MATA  <15,15)  =  1.0 
MATA(15,16  =  RBG2 ( 2 ) 

MATA( 15 , 17 )  =  -RBG2 ( 1 ) 

MATB (15)  =  MKC1  -  MKC2 

*  SUM  OF  MOMENTS  EQUATIONS  FOR  LINK  TWO  IN  THE  X,Y,Z  DIRECTIONS 

MATA( 16,11)  =  RBG2 { 3 ) 

MATA( 16 ,12)  =  -RBG2(2) 

MATA (16,16)  =  -(IXX(2 . 1)  +  IXX(2,2)) 

MATA( 16,17)  =  IXY(2,l)  +  IXY(2,2) 

MATA  16,18)  =  IXZ ( 2 , 1 )  +  IXZ(2,2) 

MATA  16,20)  =  -RAG2  3) 

MATA  16,21)  =  RAG2 ( 2 ) 

MATB ( 16)  =  -T1X  +  T2X 
IF (RUN  .EQ.  2)  GO  TO  11 

MATA(17 , 10)  =  -RBG2 ( 3 ) 

MATA (17,12)  =  RBG2 ( 1 ) 

MATA  17,16  =  I XY ( 2 , 1 )  +  IXY(2,2) 

MATA  17,17  =  - ( IYY (2.1)  +  IYY(2,2)) 

MATA(17,18  =  IYZ (2,1)  +  IYZ(2,2) 

MATA  17,19)  =  RAG2 ( 3 ) 

MATA( 17,21)  =  -RAG2 ( 1 ) 

MATB (17)  =  -  T1Y  +  T2Y 

MATA (18, 10)  =  RBG2(2) 

MATA ( 18 , 1 1 )  =  -RBG2 ( 1 ) 

MATA  18,16)  =  IXZ(2 , 1 )  +  IXZ(2,2) 

MATA( 18 , 17 )  =  IYZ ( 2 , 1 )  +  IYZ(2,2) 

MATA  18,18)  =  - ( IZZ( 2 , 1 )  +  IZZ(2,2)) 

MATA ( 18 , 19 )  =  -RAG2 ( 2 ) 

MATA( 18 , 20 )  =  RAG2 ( 1 ) 

MATB (18)  =  -  T1Z  +  T2Z 

IF  (RUN  .EQ.  3)  GO  TO  4 
11  MATA( 17,17)  =  1.0 

MATA( 18,18)  =  1.0 

*  LINK  THREE 

*  SUM  OF  FORCES  IN  THE  X  DIRECTION 

4  MATA( 19,19)  =  1.0 

MATA( 19 , 22 )  =  MASS3 

SUM  OF  FORCES  IN  THE  Y  DIRECTION 
MATA ( 20 , 20 )  =1.0 
MATA(20,23)  =  MASS3 

SUM  OF  FORCES  IN  THE  Z  DIRECTION 
MATA (21 , 21 )  =  1.0 
MATA(21,24)  =  MASS3 

MATB (21)  =  -W3 
EQUATIONS  AT  JOINT  TWO 
IN  THE  X  DIRECTION 

MATA(22 , 13)  =  -1.0 
MATA (22 , 17 )  =  -RAG2(3) 

MATA( 22 , 18 )  =  RAG2(2) 

MATA (22, 22  =  1.0 

MATA( 22 , 26 )  =  RBG3(3) 

MATA(22 , 27 )  =  -RBG3(2) 

MATB (22)  =  MIC3  -  MIC4 

IN  THE  Y  DIRECTION 

MATA( 23 ,14)  =  -1.0 
MATA( 23 , 16 )  =  RAG2 ( 3 ) 


MATA{23 ,18! 
MATA  23,23 
MATA (2 3, 2 5 
MATA(23 , 27 ; 


-RAG2 ( 1 ) 
1.0 

-RBG3 ( 3 ) 
RBG3 ( 1 ) 


MATB(23)  =  MJC3  -  MJC4 
IN  THE  Z  DIRECTION 


MATA (24,15) 
MATA  24,16 
MATA  24,17 
MATA (24 , 24 
MATA  24,25 
MATA( 24 , 26 , 


=  -1.0 
=  -RAG2(2) 
=  RAG2 ( 1 ) 

=  1.0 

=  RBG3 ( 2 ) 

=  -RBG3 ( 1 ) 


MATB(24)  =  MKC3  -  MKC4 

SUM  OF  MOMENTS  EQUATIONS  FOR  LINK  THREE  IN  THE  X,Y,Z  DIRECTIONS 
MATA(25 , 20  V  =  RBG3(3) 


MATA( 25 ,21  ,  . 

MATA  25,25)  =  -(IXX(3,1)  +  IXX(3.2)) 
MATA  25,26  =  IXY(3,1  +  IXY(3,2) 

MATA  25,27)  =  IXZ(3,1)  +  IXZ(3,2) 

MATB ( 25 )  =  -  T2X 

IF (RUN  .EQ.  1  .OR.  RUN  .EQ.  2)  GO  TO  12 


MATA( 25 , 26 
MATA  25,27 


=  -RBG3 ( 2 ) 

=  -(IXX(3,i; 


MATA ( 26 , 19 )  = 
MATA  26,21  = 
MATA (26, 25)  = 
MATA  26,26  = 
MATA( 26 , 27 )  = 

MATB (26)  =  -  1 

MATA (27,19)  = 
MATA  27,20 )  = 
MATA  27,25  = 
MATA  27,26  = 
MATA  ( 27,27)  = 


-RBG3 ( 3 ) 
RBG3 ( 1 ) 

IXY ( 3 , 1 )  + 


IXY( 3,2) 


-  ( I YY  ( 3 . 1 )  +  IYY (3,2)) 
IYZ (3,1)  +  IYZ( 3,2) 


RBG3 ( 2 ) 
-RBG3 ( 1 ) 
IXZ( 3 , 1 )  + 
IYZ  3  ,  1  )  + 
-  ( I ZZ (3,1) 


IXZ( 3 , 2) 
IYZ( 3.2) 

+  IZZ (3,2)) 


MATB (27)  = 


IF  (RUN  .EQ.  3) 
MATA ( 26.26)  -  l.C 
MATA( 27.27)  =  1 


EQ.  3)  GO  TO  13 
=  1.0 
7 )  =  1.0 


*  CALL  EQUATION  SOLVER  PROGRAM  FROM  IMSL 

13  CALL  LEQT2F ( MATA . M , N , I A , MATB  I  DOT , WKAREA , IER ) 
IF  ( IER  .NE .  0)  CALL  ENDJOB 

*  FIND  LCOGX , LCOGY , LCOGZ , THETA  VALUES . WX . WY . W2 

I F ( RUN  .EQ.  1)  GO  TO  6 
IF  (RUN  EQ.  2)  GO  TO  9 

*  LINK  ONE 

AX1  =  MATB ( 4 ' 

VELX1  =  I NT Op L 1  )  AXi 
LCOGX I  =  INTGRL  XI  VELX1  ; 

LCOGX' 1)  =  LCOGX 1 
AY  1  =  MATB  c 
■  FI  :  -  1 N T  * P I  AVI 

;  '  gy i  =  in:  i pl  v:  vfly  1 
1  1  ~  1  ■  JY 1 

AC.  -  MATB  ■: 

VF.LZ .  -  IN:  TP’  AC. 

•  *  ■  vf  I  .*. 


wt  IX  -  MA I P  ’ 


I  U  I-LIJ  I 


COS(THEXRl) 


W1X  =  INTGRL(0 ,  WD1X) 

THEXR1  =  INTGRL(TY1,W1X) 

JXQ=  LCOGX(l)  -  L ( 1 , 1 )  *  COS(TXl) 
WDX(l)  =  WD1X 
WX(1)  =  W1X 

CTHETX(l)  =  THEXR1  *  RADEG 
ETHETY(l)  =  CTHETX(l) 

WD1Y  =  MATB(8) 

W1Y  =  INTGRL(0 ,WD1Y) 

THEYR1  =  INTGRL ( 0. ,W1Y) 

JYO  =  LCOGY(l)  -  L(1 , 1)  *  COS (THEXR1) 
WDY(l)  =  WD1Y 
WY  ( 1 )  =  W1Y 

CTHETY(l)  =  THEYR1  *  RADEG 
WD1Z  =  MATB{9) 

W1Z  =  INTGRL(0 , WD1Z) 

THEZR1  =  IMTGRL(0. ,W1Z) 

WDZ(l)  =  WD1Z 
WZ(1)  =  W1Z 

CTHETZ ( 1 )  =  THEZR1  *  RADEG 
ETHETZ(l)  =  90.0  -  CTHETX(l) 

ETHEZ1  =  ETHETZ(l)  *  DEGRA 

JZO  =  LCOGZ(l)  -  L(l,l)  *  COS (ETHEZ1 ) 

LINK  TWO 

AX2  =  MATB (13) 

VELX2  =  INTGRL(0. ,AX2) 

LCOGX2  =  INTGRL(X2 ,VELX2) 

LCOGX( 2 )  =  LCOGX2 
AY 2  =  MATE (14) 

VELY2  =  INTGRL(0. ,AY2) 

LCOGY2  =  INTGRL(Y2 , VELY2) 

LCOGY ( 2 )  =  LCOGY2 
AZ2  =  MATB (15) 

VELZ2  =  INTGRL( 0 . ,AZ2) 

LCOGZ2  =  INTGRL(Z2 ,VELZ2) 

LCOGZ ( 2 )  =  LCOGZ2 
WD2X  =  MATB ( 16 ) 

W2X  =  INTGRL ( 0 . ,WD2X) 

THEXR2  =  INTGRL (TY2.W2X) 

JX1  =  LCOGX( 2 )  -  L ( 2 , 1 )  *  COS(TX2) 
WDX'2)  =  WD2X 
WX( 2 )  =  W2X 

CTHETX( 2 )  =  THEXR2  *  RADEG 
ETHETY ( 2 )  =  CTHETX(2) 

WD2Y  =  MATB (17) 

W2Y  =  INTGRL ( 0. ,WD2Y) 

THEYR2  =  INTGRL ( 0 . ,W2Y) 

JY1  =  LCOGY ( 2 )  -  L ( 2 , 1 )  *  COS(THEXR2) 
WDY ( 2 )  =  WD2Y 
WY  <  2 )  =  W2Y 

CTHETY ( 2  )  =THEYR2  *  RADEG 
WD2Z  =  MATB( 18) 

W2Z  =  INTGRL <  0 . ,WD2Z^ 

THEZR2  =  INTGRUO.  ,W2Z) 

WDZ (2)  =  WD2Z 
WZ ( 2 )  =  W2Z 

CTHETZ ( 2 )  =  THEZF2  *  RADEG 
ETHETZi 2 )  =  90.7  -  CTHETX ( 2 ) 

ETHEZ2  =  ETHETZ  f  2  )  *  DEGRA 

JZ1  =  L 0002(2)  -  L ( 2  .  1  )  *  COS ( ETHEZ2 

LINK  THREE 

AX  ■  -  MATB  2  2 

VELXi  =  INT  »FL  AX  2, 

L  XT5  =  INT  >FL  '  X  3  VELX3  i 

-i 1  v,  jA  3 

AY :  -  MATE  .  : 

YELY •  -  INT, PL  AY 


*  COS ( THEXR2 ) 


LC0GY3  =  INTGRL(Y3,VELY3) 

LC0GY{3)  =  LC0GY3 
AZ3  =  MATB(24) 

VELZ3  =  INTGRL(0 . , AZ3) 

LC0GZ3  =  INTGRL ( Z3 , VELZ3 ) 

LC0GZ(3)  =  LC0GZ3 
WD3X  =  MATB(25) 

W3X  =  INTGRL ( 0. ,WD3X) 

THEXR3  =  INTGRL(TY3.W3X) 

JX2  =  LCOGX(3 )  -  L(3 , 1 )  *  C0S(TX3) 

WDX(3 )  =  WD3X 
WX(3)  =  W3X 

CTHETX(3 )  =  THEXR3  *  RADEG 
ETHETY(3)  =  CTHETX(3) 

WD3Y  =  MATB(26) 

W3Y  =  INTGRL ( 0. ,WD3Y) 

THEYR3  =  INTGRL(0 . ,W3Y) 

JY2  =  LCOGY ( 3 )  -  L(3,l)  *  C0S(THEXR3) 

WDY ( 3 )  =  WD3Y 
WY(3)  =  W3Y 

CTHETY ( 3 )  =  THEYR3  *  RADEG 
WD3Z  =  MATB(27 ) 

W3Z  =  INTGRL ( 0. ,WD3Z) 

THEZR3  =  INTGRL (0 . ,  W3Z) 

WDZ(3)  =  WD3Z 
WZ(3)  =  W3Z 

CTHETZ(3)  =  THEZR3  *  RADEG 
ETHETZ(3)  =  90.0  -  CTHETX(3) 

ETHEZ3  =  ETHETZ(3 )  *  DEGRA 

JZ2  =  LCOGZ{3)  -  L(3 , 1 )  *  COS(ETHEZ3) 

DYNAMIC 

*  COMPUTE  THEORITICAL  TORQUE, T1X  AND  T2X 

Y  =  L(3 , 1 )  *  COS (THEXR3 ) 

Z  =  L(3, 1)  *  SIN(THEXR3 ) 

FZ2  =-MASS3*AZ3 
FY2  =  -MASS3  *  AY3 
FZ1  =  FZ2  -  MASS2  *  AZ2 
FY1  =  FY2  -  MASS2  *  kY2 

TORY2X  =  (MASS3  *  L ( 3 . 2 } **2 )*WDX( 3 )  - (FZ2  *  Y)+  (FY2  *  Z) 
TORY1X  =  (MASS2*L(2,l)**2)*WDX(2)+TORY2X-FZl*COS(THEXR2) . 
*L (2,1) +FY1*SIN ( THEXR2 ) *L{ 2 , 1 ) -FZ2*L (2,2) *COS ( THEXR2 ) +FY2 
*SIN(THEXR2 ) *L ( 2 , 2 ) 

*  COMPUTE  ERROR  BETWEEN  COMPUTED  AND  INPUTEDVALUES  OF  TORQUE  AT 

*  JOINT  ONE  AND  TWO 

ERRT2X  =  ( (TORY2X-T2X)/  4.7553)  *  100. 

ERRT1X  =  ( (TORY1X-T1X ! /  4.7553)  *  100. 

END 

STOP 

FORTRAN 

*  SUBROUTINE  TO  COMPUTE  THE  CROSS  PRODUCT  OF  TWO  VECTORS 

SUBROUTINE  CPROD ( VECTA , VECTB ,MI , MJ , MK) 

IMPLICIT  REAL*8  (A-Z) 

DIMENSION  VECTA( 3 ) , VECTB ( 3 ) 

MI  =  VECTA ( 2 )  *  VECTB ( 3 )  -  VECTA(3)  *  VECTB(2) 

MJ  =  VECTA  3)  *  VECTB  1  -  VECTA  1)  *  VECTB(3) 

MK  =  VECTA(l)  *  VECTB (2)  -  VECTA(2)  *  VECTB(l) 

RETURN 

END 
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